diff --git a/.env b/.env index 9d9a1e96e..4a95be83b 100644 --- a/.env +++ b/.env @@ -1,6 +1,6 @@ # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml PROJECT_NAME="airstack" -PROJECT_VERSION="0.12.0" +PROJECT_VERSION="0.13.0" # can replace with your docker hub username PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared" DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd" diff --git a/common/inputrc b/common/inputrc new file mode 100644 index 000000000..230e66b8f --- /dev/null +++ b/common/inputrc @@ -0,0 +1,67 @@ +# /etc/inputrc - global inputrc for libreadline +# See readline(3readline) and `info rluserman' for more information. + +# Be 8 bit clean. +set input-meta on +set output-meta on + +# To allow the use of 8bit-characters like the german umlauts, uncomment +# the line below. However this makes the meta key not work as a meta key, +# which is annoying to those which don't need to type in 8-bit characters. + +# set convert-meta off + +# try to enable the application keypad when it is called. Some systems +# need this to enable the arrow keys. +# set enable-keypad on + +# see /usr/share/doc/bash/inputrc.arrows for other codes of arrow keys + +# do not bell on tab-completion +# set bell-style none +# set bell-style visible + +# some defaults / modifications for the emacs mode +$if mode=emacs + +# allow the use of the Home/End keys +"\e[1~": beginning-of-line +"\e[4~": end-of-line + +# allow the use of the Delete/Insert keys +"\e[3~": delete-char +"\e[2~": quoted-insert + +# mappings for "page up" and "page down" to step to the beginning/end +# of the history +# "\e[5~": beginning-of-history +# "\e[6~": end-of-history + +# alternate mappings for "page up" and "page down" to search the history +"\e[5~": history-search-backward +"\e[6~": history-search-forward + +# mappings for Ctrl-left-arrow and Ctrl-right-arrow for word moving +"\e[1;5C": forward-word +"\e[1;5D": backward-word +"\e[5C": forward-word +"\e[5D": backward-word +"\e\e[C": forward-word +"\e\e[D": backward-word + +$if term=rxvt +"\e[7~": beginning-of-line +"\e[8~": end-of-line +"\eOc": forward-word +"\eOd": backward-word +$endif + +# for non RH/Debian xterm, can't hurt for RH/Debian xterm +# "\eOH": beginning-of-line +# "\eOF": end-of-line + +# for freebsd console +# "\e[H": beginning-of-line +# "\e[F": end-of-line + +$endif diff --git a/common/ros_packages/bag_recorder_pid/.gitignore b/common/ros_packages/bag_recorder_pid/.gitignore new file mode 100644 index 000000000..e10affe1e --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ +health_monitor/health_monitor/__pycache__ +health_monitor/__pycache__/* +*.pyc +.vscode/ +logging/ diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/common/ros_packages/bag_recorder_pid/bag_record_pid/__init__.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py rename to common/ros_packages/bag_recorder_pid/bag_record_pid/__init__.py diff --git a/common/ros_packages/bag_recorder_pid/bag_record_pid/bag_record_node.py b/common/ros_packages/bag_recorder_pid/bag_record_pid/bag_record_node.py new file mode 100644 index 000000000..9de500b34 --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/bag_record_pid/bag_record_node.py @@ -0,0 +1,203 @@ + +import copy +from rclpy.node import Node +from pathlib import Path +from std_msgs.msg import Bool +import yaml +import subprocess +import signal +import time +import os +import rclpy +from rclpy.qos import QoSProfile, QoSReliabilityPolicy +import datetime + +class BagRecorderNode(Node): + def __init__(self): + super().__init__("bag_record_node") + + self.node_name = self.get_name() # Get the full node name, including namespace if any + + self.declare_parameter( + "cfg_path", str(Path(__file__).parents[3] / "config/cfg.yaml") + ) + + self.declare_parameter( + "output_dir", str("/logging/") + ) + + self.declare_parameter( + "mcap_qos_dir", "" + ) + + self.cfg_path = self.get_parameter("cfg_path").get_parameter_value().string_value + self.output_dir = self.get_parameter("output_dir").get_parameter_value().string_value + self.mcap_qos_dir = self.get_parameter("mcap_qos_dir").get_parameter_value().string_value + + self.active = False + self.cfg = yaml.safe_load(open(self.cfg_path)) + + # TODO: check if the output directory exists. + # Exit if it does not exist. + os.chdir(self.output_dir) + + self.command_prefix = ["ros2", "bag", "record", "-s", "mcap"] + self.commands = dict() + self.add_topics() + + self.process = dict() + + # Create QoS profile for reliable communication + reliable_qos = QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + depth=10 + ) + + # Use reliable QoS for the status publisher + self.status_pub = self.create_publisher( + Bool, + f"{self.node_name}/bag_recording_status", + reliable_qos + ) + + self.toggle_status = self.create_subscription( + Bool, + f"{self.node_name}/set_recording_status", + self.set_status_callback, + 10 + ) + + self.timer = self.create_timer(0.5, self.pub_status_callback) + + def add_topics(self): + '''The configuration file looks like + + sections: + gps_lidar_spot_depth_status: + mcap_qos: mcap_qos.yaml + args: + - -b + - 4000000000 # ~4GB + - --max-cache-size + - 1073741824 # 1GB + topics: + - /tf + - gq7/ekf/llh_position + + The -o or --output argument should not be specified here. + The "mcap_qos" field here will be interpreted as the filename of the MCAP QoS profile. + + self.commands[section_name] = { + 'prefix': [], + 'suffix': [], + } + ''' + namespace = self.get_namespace() + + for section_name, section_config in self.cfg['sections'].items(): + self.commands[section_name] = dict() + + # Command lists. + self.commands[section_name]['prefix'] = [] + self.commands[section_name]['suffix'] = [] + + # Populate the initial command line. + self.commands[section_name]['prefix'].extend(self.command_prefix) + + # Add the args to the command line. + str_args = [ str(c) for c in section_config['args'] ] + self.commands[section_name]['prefix'].extend(str_args) + + # Set the mcap qos profile. + if section_config['mcap_qos'] != "": + mcap_qos_path = os.path.join(self.mcap_qos_dir, str(section_config['mcap_qos'])) + self.commands[section_name]['prefix'].append('--storage-config-file') + self.commands[section_name]['prefix'].append(mcap_qos_path) + + self.get_logger().warn( + f'CMD for section {section_name}: ' + f'{" ".join(self.commands[section_name]["prefix"])}' ) + + # Add the topics to the command at the end. + self.get_logger().warn(f"Recording section {section_name} topics:") + if 'exclude' in section_config.keys(): + if 'topics' in section_config.keys(): + self.get_logger().error('Cannot mix exclude with topics.') + exit() + + self.commands[section_name]['suffix'].append('--all') + for topic in section_config['exclude']: + self.commands[section_name]['suffix'].append('--exclude') + self.commands[section_name]['suffix'].append(topic) + self.get_logger().info(str(self.commands[section_name])) + else: + for topic in section_config['topics']: + if topic.startswith('/'): + full_topic_name = topic + else: + full_topic_name = f"{namespace}/{topic}" + + self.commands[section_name]['suffix'].append(full_topic_name) + self.get_logger().warn(f"{full_topic_name}") + + def pub_status_callback(self): + msg = Bool() + msg.data = self.active + self.status_pub.publish(msg) + + def set_status_callback(self, msg): + if msg.data: + self.run() + else: + self.interrupt() + + def run(self): + if not self.active: + self.active = True + + time_suffix = f"{datetime.datetime.now().strftime('%Y%m%d_%H%M%S')}" + + for section_name, command_dict in self.commands.items(): + cmd = copy.deepcopy(command_dict['prefix']) + + # Set the output filename. + output_filename = f"{section_name}_{time_suffix}" + cmd.append('-o') + cmd.append(output_filename) + + # Appending an empty string will cause the ros2 bag record to consider the space as a topic + # and introduce an error. + if len(command_dict['suffix']) > 0: + cmd.extend(command_dict['suffix']) + + self.get_logger().warn(f"Running command: {' '.join(cmd)}") + #self.get_logger().warn(f"Running command: {cmd}") + + self.process[section_name] = dict() + self.process[section_name]['process'] = subprocess.Popen(cmd) + self.process[section_name]['pid'] = self.process[section_name]['process'].pid + self.process[section_name]['output_filename'] = output_filename + self.get_logger().warn(f"Started Recording Section {section_name} with PID {self.process[section_name]['pid']} to {output_filename}") + + def interrupt(self): + if self.active: + for section_name, process in self.process.items(): + process['process'].send_signal(signal.SIGINT) + process['process'].wait() + self.get_logger().info(f"Ending Recording of Section {section_name} with PID {process['pid']}") + self.get_logger().warn(f"Output filename: {process['output_filename']}") + self.active = False + + +def main(args=None): + rclpy.init(args=args) + node = BagRecorderNode() + rclpy.spin(node) + node.interrupt() + + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == "__main__": + main() diff --git a/common/ros_packages/bag_recorder_pid/config/cfg.yaml b/common/ros_packages/bag_recorder_pid/config/cfg.yaml new file mode 100644 index 000000000..d6836a9cd --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/config/cfg.yaml @@ -0,0 +1,2 @@ +topics: + ["/a", "/b"] \ No newline at end of file diff --git a/common/ros_packages/bag_recorder_pid/config/example-exclude.yaml b/common/ros_packages/bag_recorder_pid/config/example-exclude.yaml new file mode 100644 index 000000000..ee4c20eec --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/config/example-exclude.yaml @@ -0,0 +1,47 @@ +# Any line that does not start with a / will be automatically prefixed using the +# namespace of the bag_record_pid node. +# mcap_qos is the filename of the MCAP QoS profile. The actual directory will be prefixed by the +# bag_record_pid node via a user specified argument. All MCAP QoS files must be in the same directory. +# The -o or --output argument should not be specified here. +# The -s mcap option will be added automatically. +sections: + spot_rgb: + mcap_qos: mcap_qos.yaml + args: + - -b + - 4000000000 # ~4GB + - --max-cache-size + - 1073741824 # 1GB + exclude: + - /tf + - /tf_static + +# Extra or skipped topics: +# - spot/depth_registered/back/points +# - spot/depth_registered/frontleft/points +# - spot/depth_registered/frontright/points +# - spot/depth_registered/left/points +# - spot/depth_registered/right/point +# - spot/depth_registered/back/camera_info +# - spot/depth_registered/back/image +# - spot/depth_registered/frontleft/camera_info +# - spot/depth_registered/frontleft/image +# - spot/depth_registered/frontright/camera_info +# - spot/depth_registered/frontright/image +# - spot/depth_registered/hand/points +# - spot/depth_registered/hand/camera_info +# - spot/depth_registered/hand/image +# - spot/depth_registered/left/camera_info +# - spot/depth_registered/left/image +# - spot/depth_registered/right/camera_info +# - spot/depth_registered/right/image +# ouster/scan +# spot/camera/back/compressed +# spot/camera/frontleft/compressed +# spot/camera/frontright/compressed +# spot/camera/hand/compressed +# spot/camera/left/compressed +# spot/camera/right/compressed +# spot/depth_registered/image_rect/compressed +# spot/depth_registered/image_rect/compressedDepth +# spot/depth_registered/image_rect/theora \ No newline at end of file diff --git a/common/ros_packages/bag_recorder_pid/config/example-spot1-auto.yaml b/common/ros_packages/bag_recorder_pid/config/example-spot1-auto.yaml new file mode 100644 index 000000000..cc32168be --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/config/example-spot1-auto.yaml @@ -0,0 +1,130 @@ +# Any line that does not start with a / will be automatically prefixed using the +# namespace of the bag_record_pid node. +# mcap_qos is the filename of the MCAP QoS profile. The actual directory will be prefixed by the +# bag_record_pid node via a user specified argument. All MCAP QoS files must be in the same directory. +# The -o or --output argument should not be specified here. +# The -s mcap option will be added automatically. +sections: + gps_lidar_spot_depth_status: + mcap_qos: mcap_qos.yaml + args: + - -b + - 4000000000 # ~4GB + - --max-cache-size + - 1073741824 # 1GB + topics: + - /parameter_events + - /rosout + - /tf + - /tf_static + - bag_record_pid/bag_recording_status + - bag_record_pid/set_recording_status + - gq7/ekf/llh_position + - gq7/ekf/odometry_earth + - gq7/ekf/odometry_map + - gq7/ekf/status + - gq7/gnss_1/llh_position + - gq7/gnss_2/llh_position + - gq7/mip/ekf/status + - gq7/mip/gnss_1/fix_info + - gq7/mip/gnss_2/fix_info + - gq7/nmea + - gq7/rtcm + - gq7/gnss_1/time + - gq7/gnss_2/time + - gq7/imu/data + - gq7/imu/data_raw + - gq7/mip/gnss_corrections/rtk_corrections_status + - is_inside_geofence + - ouster/imu + - ouster/points + - ouster/metadata + - ouster/os_driver/transition_event + - ouster/telemetry + - ouster/nearir_image + - ouster/range_image + - ouster/reflec_image + - ouster/signal_image + - spot/arm_joint_commands + - spot/arm_pose_commands + - spot/body_pose + - spot/cmd_vel + - spot/joint_states + - spot/manipulation_state + - spot/odometry + - spot/odometry/twist + - spot/robot_description + - spot/status/battery_states + - spot/status/behavior_faults + - spot/status/end_effector_force + - spot/status/estop + - spot/status/feedback + - spot/status/feet + - spot/status/leases + - spot/status/metrics + - spot/status/mobility_params + - spot/status/power_states + - spot/status/system_faults + - spot/status/wifi + - spot/depth/back/camera_info + - spot/depth/back/image + - spot/depth/frontleft/camera_info + - spot/depth/frontleft/image + - spot/depth/frontright/camera_info + - spot/depth/frontright/image + - spot/depth/hand/camera_info + - spot/depth/hand/image + - spot/depth/left/camera_info + - spot/depth/left/image + - spot/depth/right/camera_info + - spot/depth/right/image + spot_rgb: + mcap_qos: mcap_qos.yaml + args: + - -b + - 4000000000 # ~4GB + - --max-cache-size + - 1073741824 # 1GB + topics: + - spot/camera/back/camera_info + - spot/camera/back/image + - spot/camera/frontleft/camera_info + - spot/camera/frontleft/image + - spot/camera/frontright/camera_info + - spot/camera/frontright/image + - spot/camera/hand/camera_info + - spot/camera/hand/image + - spot/camera/left/camera_info + - spot/camera/left/image + - spot/camera/right/camera_info + - spot/camera/right/image + +# Extra or skipped topics: +# - spot/depth_registered/back/points +# - spot/depth_registered/frontleft/points +# - spot/depth_registered/frontright/points +# - spot/depth_registered/left/points +# - spot/depth_registered/right/point +# - spot/depth_registered/back/camera_info +# - spot/depth_registered/back/image +# - spot/depth_registered/frontleft/camera_info +# - spot/depth_registered/frontleft/image +# - spot/depth_registered/frontright/camera_info +# - spot/depth_registered/frontright/image +# - spot/depth_registered/hand/points +# - spot/depth_registered/hand/camera_info +# - spot/depth_registered/hand/image +# - spot/depth_registered/left/camera_info +# - spot/depth_registered/left/image +# - spot/depth_registered/right/camera_info +# - spot/depth_registered/right/image +# ouster/scan +# spot/camera/back/compressed +# spot/camera/frontleft/compressed +# spot/camera/frontright/compressed +# spot/camera/hand/compressed +# spot/camera/left/compressed +# spot/camera/right/compressed +# spot/depth_registered/image_rect/compressed +# spot/depth_registered/image_rect/compressedDepth +# spot/depth_registered/image_rect/theora \ No newline at end of file diff --git a/common/ros_packages/bag_recorder_pid/config/mcap_qos.yaml b/common/ros_packages/bag_recorder_pid/config/mcap_qos.yaml new file mode 100644 index 000000000..ec933aaef --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/config/mcap_qos.yaml @@ -0,0 +1,8 @@ +noChunkCRC: false +noChunking: false +noMessageIndex: false +noSummary: false +chunkSize: 10485760 # 10MB +compression: "Zstd" +compressionLevel: "Fastest" +forceCompression: false \ No newline at end of file diff --git a/common/ros_packages/bag_recorder_pid/launch/bag_record_pid.launch.py b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid.launch.py new file mode 100644 index 000000000..63b13b405 --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid.launch.py @@ -0,0 +1,50 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Get the package directory + pkg_dir = get_package_share_directory('bag_record_pid') + + # Declare launch arguments + cfg_path_arg = DeclareLaunchArgument( + 'cfg_path', + default_value=LaunchConfiguration('cfg_path', default=pkg_dir + '/config/cfg.yaml'), + description='Configuration file for bag record pid' + ) + + output_dir_arg = DeclareLaunchArgument( + 'output_dir', + default_value=LaunchConfiguration('output_dir', default='/logging'), + description='Logging directory' + ) + + mcap_qos_dir_arg = DeclareLaunchArgument( + 'mcap_qos_dir', + default_value=LaunchConfiguration('mcap_qos_dir', default=pkg_dir + '/config'), + description='MCAP QoS directory' + ) + + # Include the bag record node launch file + bag_record_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + get_package_share_directory('bag_record_pid'), + '/launch/bag_record_pid_node.launch.py' + ]), + launch_arguments={ + 'cfg_path': LaunchConfiguration('cfg_path'), + 'output_dir': LaunchConfiguration('output_dir'), + 'mcap_qos_dir': LaunchConfiguration('mcap_qos_dir') + }.items() + ) + + return LaunchDescription([ + # Launch arguments + cfg_path_arg, + output_dir_arg, + mcap_qos_dir_arg, + # Include launch file + bag_record_launch + ]) diff --git a/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_namespaced.launch.py b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_namespaced.launch.py new file mode 100644 index 000000000..180f29f9f --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_namespaced.launch.py @@ -0,0 +1,72 @@ +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + GroupAction, +) + +from launch_ros.actions import PushRosNamespace +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + # Get the path to the savior_database package + pkg_dir = get_package_share_directory('bag_record_pid') + + # Add robot_prefix launch argument + robot_prefix_arg = DeclareLaunchArgument( + 'robot_prefix', + default_value='robot1', + description='Prefix/namespace for the robot' + ) + + # Declare all launch arguments + cfg_path_arg = DeclareLaunchArgument( + 'cfg_path', + default_value=os.path.join(pkg_dir, 'config', 'cfg.yaml'), + description='Configuration file for bag record pid' + ) + + mcap_qos_dir_arg = DeclareLaunchArgument( + 'mcap_qos_dir', + default_value=os.path.join(pkg_dir, 'config'), + description='MCAP QoS directory' + ) + + output_dir_arg = DeclareLaunchArgument( + 'output_dir', + default_value='/logging', + description='Logging directory' + ) + + # Create a group action with namespace + bag_record_group = GroupAction( + actions=[ + # Push ROS namespace + PushRosNamespace(LaunchConfiguration('robot_prefix')), + # Include the bag_record_pid node launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_dir, 'launch', 'bag_record_pid_node.launch.py') + ), + launch_arguments={ + 'cfg_path': LaunchConfiguration('cfg_path'), + 'output_dir': LaunchConfiguration('output_dir'), + 'mcap_qos_dir': LaunchConfiguration('mcap_qos_dir') + }.items() + ) + ] + ) + + return LaunchDescription([ + # Add robot_prefix argument + robot_prefix_arg, + # Add all argument declarations + cfg_path_arg, + mcap_qos_dir_arg, + output_dir_arg, + # Include the group containing the database node launch file + bag_record_group + ]) diff --git a/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_node.launch.py b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_node.launch.py new file mode 100644 index 000000000..f31865383 --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/launch/bag_record_pid_node.launch.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + pkg_dir = get_package_share_directory('bag_record_pid') + + # Declare all launch arguments + cfg_path_arg = DeclareLaunchArgument( + 'cfg_path', + default_value=os.path.join(pkg_dir, 'config', 'cfg.yaml'), + description='Configuration file for bag record pid' + ) + + output_dir_arg = DeclareLaunchArgument( + 'output_dir', + default_value='/logging', + description='Logging directory' + ) + + mcap_qos_dir_arg = DeclareLaunchArgument( + 'mcap_qos_dir', + default_value=os.path.join(pkg_dir, 'config'), + description='MCAP QoS directory' + ) + + # Create the node with launch configurations + bag_record_node = Node( + package='bag_record_pid', + executable='bag_record_node', + name='bag_record_pid', + parameters=[{ + 'cfg_path': LaunchConfiguration('cfg_path'), + 'output_dir': LaunchConfiguration('output_dir'), + 'mcap_qos_dir': LaunchConfiguration('mcap_qos_dir') + }], + output='screen' + ) + + return LaunchDescription([ + # Add all argument declarations + cfg_path_arg, + output_dir_arg, + mcap_qos_dir_arg, + # Add the node + bag_record_node + ]) + diff --git a/common/ros_packages/bag_recorder_pid/package.xml b/common/ros_packages/bag_recorder_pid/package.xml new file mode 100644 index 000000000..463396d06 --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/package.xml @@ -0,0 +1,18 @@ + + + + bag_record_pid + 0.0.0 + TODO: Package description + airlab + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/rqt_behavior_tree b/common/ros_packages/bag_recorder_pid/resource/bag_record_pid similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/rqt_behavior_tree rename to common/ros_packages/bag_recorder_pid/resource/bag_record_pid diff --git a/common/ros_packages/bag_recorder_pid/setup.cfg b/common/ros_packages/bag_recorder_pid/setup.cfg new file mode 100644 index 000000000..9956a016a --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/bag_record_pid +[install] +install_scripts=$base/lib/bag_record_pid diff --git a/common/ros_packages/bag_recorder_pid/setup.py b/common/ros_packages/bag_recorder_pid/setup.py new file mode 100644 index 000000000..36c6ce8aa --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/setup.py @@ -0,0 +1,34 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'bag_record_pid' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Include all launch files. + # https://docs.ros.org/en/humble/Tutorials/Intermediate/Launch/Launch-system.html + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + # Include all resource files. + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='airlab', + maintainer_email='mmayank74567@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'bag_record_node = bag_record_pid.bag_record_node:main' + ], + }, +) diff --git a/common/ros_packages/bag_recorder_pid/test/test_copyright.py b/common/ros_packages/bag_recorder_pid/test/test_copyright.py new file mode 100644 index 000000000..97a39196e --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/common/ros_packages/bag_recorder_pid/test/test_flake8.py b/common/ros_packages/bag_recorder_pid/test/test_flake8.py new file mode 100644 index 000000000..27ee1078f --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/common/ros_packages/bag_recorder_pid/test/test_pep257.py b/common/ros_packages/bag_recorder_pid/test/test_pep257.py new file mode 100644 index 000000000..b234a3840 --- /dev/null +++ b/common/ros_packages/bag_recorder_pid/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/.gitignore b/common/ros_packages/rqt_behavior_tree/.gitignore similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/.gitignore rename to common/ros_packages/rqt_behavior_tree/.gitignore diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/CHANGELOG.rst b/common/ros_packages/rqt_behavior_tree/CHANGELOG.rst similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/CHANGELOG.rst rename to common/ros_packages/rqt_behavior_tree/CHANGELOG.rst diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/package.xml b/common/ros_packages/rqt_behavior_tree/package.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/package.xml rename to common/ros_packages/rqt_behavior_tree/package.xml diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/plugin.xml b/common/ros_packages/rqt_behavior_tree/plugin.xml similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/plugin.xml rename to common/ros_packages/rqt_behavior_tree/plugin.xml diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/py_console_widget.ui b/common/ros_packages/rqt_behavior_tree/resource/py_console_widget.ui similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/py_console_widget.ui rename to common/ros_packages/rqt_behavior_tree/resource/py_console_widget.ui diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/common/ros_packages/rqt_behavior_tree/resource/rqt_behavior_tree similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py rename to common/ros_packages/rqt_behavior_tree/resource/rqt_behavior_tree diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.cfg b/common/ros_packages/rqt_behavior_tree/setup.cfg similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.cfg rename to common/ros_packages/rqt_behavior_tree/setup.cfg diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.py b/common/ros_packages/rqt_behavior_tree/setup.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.py rename to common/ros_packages/rqt_behavior_tree/setup.py diff --git a/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/main.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/main.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/main.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/main.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py similarity index 82% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py index eacc3b13a..b44d75b8c 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py +++ b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py @@ -47,6 +47,7 @@ from rqt_behavior_tree.xdot.xdot_qt import DotWidget from std_msgs.msg import String +import rclpy try: from rqt_behavior_tree.spyder_console_widget import SpyderConsoleWidget @@ -69,6 +70,8 @@ def __init__(self, context): self.initialized_buttons = False self.prev_graphviz = '' + self.node = context.node + #self.behavior_tree_graphviz_sub = rospy.Subscriber('behavior_tree_graphviz', String, self.behavior_tree_graphviz_callback) #self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback) self.functions_mutex = threading.Lock() @@ -167,10 +170,18 @@ def __init__(self, context): self.button_container_widget.hide() - self.behavior_tree_graphviz_sub = context.node.create_subscription(String, - 'behavior_tree_graphviz', - self.behavior_tree_graphviz_callback, - 10) + self.behavior_tree_graphviz_sub = self.node.create_subscription(String, + 'behavior_tree_graphviz', + self.behavior_tree_graphviz_callback, + 10) + self.behavior_tree_graphviz_subs = {} + + self.selected_robot = '' + latched_qos = rclpy.qos.QoSProfile(history=rclpy.qos.HistoryPolicy.KEEP_LAST, + depth=1, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.robot_selection_sub = context.node.create_subscription(String, 'robot_selection', + self.robot_selection_callback, latched_qos) ''' self.setObjectName('PyConsole') @@ -185,6 +196,28 @@ def __init__(self, context): self._widget.windowTitle() + (' (%d)' % context.serial_number())) self._context.add_widget(self._widget) ''' + + def robot_selection_callback(self, msg): + self.selected_robot = msg.data + if msg.data not in self.behavior_tree_graphviz_subs.keys(): + def callback(m): + self.robot_graphviz_callback(msg.data, m) + self.behavior_tree_graphviz_subs[msg.data] = self.node.create_subscription(String, + '/' + msg.data + \ + '/behavior/behavior_tree_graphviz', + callback, + 10) + ''' + self.behavior_tree_graphviz_sub.destroy() + self.behavior_tree_graphviz_sub = self.node.create_subscription(String, + '/' + msg.data + '/behavior/behavior_tree_graphviz', + self.behavior_tree_graphviz_callback, + 10) + ''' + + def robot_graphviz_callback(self, robot, msg): + if robot == self.selected_robot: + self.behavior_tree_graphviz_callback(msg) def behavior_tree_graphviz_callback(self, msg): if msg.data != self.prev_graphviz: diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/test.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/test.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/test.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/test.py diff --git a/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py b/common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py similarity index 100% rename from robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py rename to common/ros_packages/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 9049fae81..88b3f5ee8 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -6,6 +6,7 @@ services: extends: file: ./ground-control-station-base-docker-compose.yaml service: ground-control-station-base + container_name: ground-control-station networks: - airstack_network ports: @@ -18,7 +19,12 @@ services: extends: file: ./ground-control-station-base-docker-compose.yaml service: ground-control-station-base + container_name: ground-control-station-real network_mode: host + volumes: + - $HOME/bags/:/bags + - ../../robot/ros_ws/src/robot_bringup/rviz/:/bags/rviz + - ../../plot:/plot # include: # - ./tak-docker-compose.yaml \ No newline at end of file diff --git a/ground_control_station/docker/ground-control-station-base-docker-compose.yaml b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml index 8e94b1053..c6783cf41 100644 --- a/ground_control_station/docker/ground-control-station-base-docker-compose.yaml +++ b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml @@ -7,7 +7,6 @@ services: dockerfile: docker/Dockerfile.gcs tags: - *gcs_image - container_name: ground-control-station entrypoint: "" command: > bash -c "ssh service restart; @@ -43,4 +42,5 @@ services: # autonomy stack stuff - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages - - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml \ No newline at end of file + - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml + - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective index d171a4005..df6f852d9 100644 --- a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective +++ b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective @@ -4,14 +4,14 @@ "mainwindow": { "keys": { "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000006400000064000005ea0000037b0000006400000089000005ea0000037b00000000000000000a000000006400000089000005ea0000037b')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000640000006400000776000003ab000000640000008900000776000003ab00000000000000000a00000000640000008900000776000003ab')", "type": "repr(QByteArray.hex)", - "pretty-print": " d d { d { d {" + "pretty-print": " d d v d v d v " }, "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000587000002c9fc0100000001fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f010000000000000587000002cd00ffffff000005870000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000713000002f9fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f01000000000000038f000002cd00fffffffb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f01000003950000037e0000007e00ffffff000007130000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", "type": "repr(QByteArray.hex)", - "pretty-print": " jrqt_ground_control_station__GroundControlStation__1__ 6MinimizedDockWidgetsToolbar " + "pretty-print": " jrqt_ground_control_station__GroundControlStation__1__ Brqt_behavior_tree__PyConsole__1__ 6MinimizedDockWidgetsToolbar " } }, "groups": { @@ -29,11 +29,33 @@ "pluginmanager": { "keys": { "running-plugins": { - "repr": "{'rqt_ground_control_station/GroundControlStation': [1]}", + "repr": "{'rqt_behavior_tree/PyConsole': [1], 'rqt_ground_control_station/GroundControlStation': [1]}", "type": "repr" } }, "groups": { + "plugin__rqt_behavior_tree__PyConsole__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + } + } + }, "plugin__rqt_ground_control_station__GroundControlStation__1": { "keys": {}, "groups": { diff --git a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py index 60e5fbbd9..f8f22b5d9 100644 --- a/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py +++ b/ground_control_station/ros_ws/src/rqt_ground_control_station/src/rqt_ground_control_station/template.py @@ -52,9 +52,11 @@ from behavior_tree_msgs.msg import Status, BehaviorTreeCommand, BehaviorTreeCommands from airstack_msgs.msg import FixedTrajectory from diagnostic_msgs.msg import KeyValue +from std_msgs.msg import String from .drag_and_drop import DragWidget, DragItem from .trajectory_dialog import TrajectoryDialog +import rclpy logger = None @@ -74,6 +76,11 @@ def __init__(self, context): self.node = self.context.node global logger logger = self.node.get_logger() + + latched_qos = rclpy.qos.QoSProfile(history=rclpy.qos.HistoryPolicy.KEEP_LAST, + depth=1, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.robot_selection_pub = self.node.create_publisher(String, 'robot_selection', latched_qos) # main layout self.widget = QWidget() @@ -88,10 +95,15 @@ def __init__(self, context): self.config_widget.setLayout(self.config_layout) self.config_widget.setFixedHeight(50) + self.pause_all_button = qt.QPushButton('PAUSE ALL ROBOTS') + self.pause_all_button.clicked.connect(self.pause_all) + self.config_layout.addWidget(self.pause_all_button) + self.robot_selection_label = qt.QLabel('Robot to command:') self.config_layout.addWidget(self.robot_selection_label) self.robot_combo_box = qt.QComboBox() + self.robot_combo_box.currentIndexChanged.connect(self.robot_selection_change) self.config_layout.addWidget(self.robot_combo_box) self.vbox.addWidget(self.config_widget) @@ -178,6 +190,28 @@ def __init__(self, context): self.timer = core.QTimer(self) self.timer.timeout.connect(self.play) + def pause_all(self): + commands = BehaviorTreeCommands() + for group in self.button_groups.keys(): + for i in range(len(self.button_groups[group]['buttons'])): + b = self.button_groups[group]['buttons'][i] + command = BehaviorTreeCommand() + command.condition_name = self.button_groups[group]['condition_names'][i] + if b.isChecked(): + b.toggle() + + command.status = Status.FAILURE + if command.condition_name == 'Pause Commanded': + command.status = Status.SUCCESS + commands.commands.append(command) + for robot in self.settings['publishers'].keys(): + self.settings['publishers'][robot]['command_pub'].publish(commands) + + def robot_selection_change(self, s): + msg = String() + msg.data = self.robot_combo_box.itemText(s) + self.robot_selection_pub.publish(msg) + def save_timeline(self): timeline_widgets = self.get_timeline_widgets() save_data = [] diff --git a/plot/nx03_traj.png b/plot/nx03_traj.png new file mode 100644 index 000000000..d75137a2a Binary files /dev/null and b/plot/nx03_traj.png differ diff --git a/plot/plot.py b/plot/plot.py new file mode 100755 index 000000000..156b7cd78 --- /dev/null +++ b/plot/plot.py @@ -0,0 +1,61 @@ +#!/usr/bin/python3 +import rosbag2_py +import numpy as np +import matplotlib.pyplot as plt +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +import sys +import math + +def plot(bag_path, odom_topic_name, tracking_point_topic_name): + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='sqlite3') + converter_options = rosbag2_py.ConverterOptions(input_serialization_format='', output_serialization_format='') + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + type_map = {topic.name: topic.type for topic in topic_types} + msg_type = get_message(type_map[odom_topic_name]) + + odoms = [] + tps = [] + yaws = [] + while reader.has_next(): + topic, data, _ = reader.read_next() + if topic == odom_topic_name: + msg = deserialize_message(data, get_message(type_map[odom_topic_name])) + odoms.append([msg.pose.pose.position.x, msg.pose.pose.position.y]) + + q = msg.pose.pose.orientation + yaw = math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) + yaws.append(yaw) + elif topic == tracking_point_topic_name: + msg = deserialize_message(data, get_message(type_map[tracking_point_topic_name])) + tps.append([msg.pose.position.x, msg.pose.position.y]) + + + odoms = np.array(odoms) + tps = np.array(tps) + + plt.figure() + plt.plot(odoms[:, 0], odoms[:, 1], marker='.', linestyle='-', label='odom') + plt.plot(tps[:, 0], tps[:, 1], marker='.', linestyle='-', label='tracking point') + + for i, yaw in enumerate(yaws): + x = odoms[i, 0] + y = odoms[i, 1] + plt.arrow(x, y, 0.3 * math.cos(yaw), 0.3 * math.sin(yaw), head_width=0.05, head_length=0.05, fc='r', ec='r') + + plt.xlabel('X Position (m)') + plt.ylabel('Y Position (m)') + plt.axis('equal') + plt.legend() + plt.show() + +if __name__ == "__main__": + bag_path = sys.argv[1] + namespace = '/' + sys.argv[2] + odom_topic_name = namespace + '/odometry_conversion/odometry' + tracking_point_topic_name = namespace + '/trajectory_controller/tracking_point' + + plot(bag_path, odom_topic_name, tracking_point_topic_name) diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot index aa614a1cc..9c0186555 100644 --- a/robot/docker/Dockerfile.robot +++ b/robot/docker/Dockerfile.robot @@ -33,6 +33,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ sudo \ software-properties-common \ wget \ + iputils-ping \ + net-tools \ && rm -rf /var/lib/apt/lists/* # Install ROS2 @@ -60,7 +62,7 @@ WORKDIR /root/ros_ws # Install dev tools RUN apt update && apt install -y \ - vim nano wget curl tree \ + vim nano emacs wget curl tree \ cmake build-essential \ less htop jq \ python3-pip \ diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml index 201c16575..635f100a5 100644 --- a/robot/docker/docker-compose.yaml +++ b/robot/docker/docker-compose.yaml @@ -56,8 +56,10 @@ services: tmux new -d -s robot_bringup && tmux send-keys -t robot_bringup 'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi; - ros2 launch robot_bringup robot.launch.xml sim:="false" ' ENTER + DATE=$(date | sed \"s/ /_/g\") ros2 launch robot_bringup robot.launch.xml sim:="false" ' ENTER && sleep infinity" runtime: nvidia # assumes network isolation via a physical router, so uses network_mode=host - network_mode: host \ No newline at end of file + network_mode: host + volumes: + - $HOME/bags:/bags:rw \ No newline at end of file diff --git a/robot/docker/robot-base-docker-compose.yaml b/robot/docker/robot-base-docker-compose.yaml index 1f9fff6c9..932657d02 100644 --- a/robot/docker/robot-base-docker-compose.yaml +++ b/robot/docker/robot-base-docker-compose.yaml @@ -28,6 +28,7 @@ services: - .dev:/root/.dev:rw # developer config - .bashrc:/root/.bashrc:rw # bash config - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name + - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history # autonomy stack stuff - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/CMakeLists.txt new file mode 100644 index 000000000..503f0e46d --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.8) +project(drone_safety_monitor) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(trajectory_library REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +add_executable(drone_safety_monitor src/drone_safety_monitor.cpp) +target_include_directories(drone_safety_monitor PUBLIC + $ + $) +target_compile_features(drone_safety_monitor PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + drone_safety_monitor + std_msgs + airstack_msgs + airstack_common + trajectory_library + mavros_msgs + std_srvs +) + +install(TARGETS drone_safety_monitor + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ + ) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/README.md b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/README.md new file mode 100644 index 000000000..ad4593933 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/README.md @@ -0,0 +1,4 @@ +### TakeoffLandingPlanner + + +Author: diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/config/takeoff_landing_planner.yaml new file mode 100644 index 000000000..23d11f151 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/config/takeoff_landing_planner.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + takeoff_height: 5.0 + high_takeoff_height: 5.0 + takeoff_landing_velocity: 0.3 + takeoff_acceptance_distance: 0.3 + takeoff_acceptance_time: 10.0 + landing_stationary_distance: 0.02 + landing_acceptance_time: 5.0 + landing_tracking_point_ahead_time: 5.0 + + takeoff_path_roll: 0. # in degrees + takeoff_path_pitch: 0. # in degrees + takeoff_path_relative_to_orientation: false diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp new file mode 100644 index 000000000..f9aee5bf1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/include/drone_safety_monitor/drone_safety_monitor.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include +#include +#include +#include +#include + +// ===================================================================================== +// ---------------------------------- TimeChecker -------------------------------------- +// ===================================================================================== + +class TimeChecker{ +private: + rclcpp::Time time; + bool time_initialized; + +public: + TimeChecker(); + void update(rclcpp::Time time); + double elapsed_since_last_update(rclcpp::Time time); +}; diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml new file mode 100644 index 000000000..7b4a46a66 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/launch/takeoff_landing_planner.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/package.xml b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/package.xml new file mode 100644 index 000000000..54c32c903 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/package.xml @@ -0,0 +1,26 @@ + + + + drone_safety_monitor + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + airstack_msgs + airstack_common + trajectory_library + mavros_msgs + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp new file mode 100644 index 000000000..c8a18e4d8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/drone_safety_monitor/src/drone_safety_monitor.cpp @@ -0,0 +1,64 @@ +#include + +TimeChecker::TimeChecker() + : time_initialized(false){ +} + +void TimeChecker::update(rclcpp::Time time){ + this->time = time; + time_initialized = true; +} + +double TimeChecker::elapsed_since_last_update(rclcpp::Time time){ + if(!time_initialized) + return std::numeric_limits::infinity(); + return (time - this->time).seconds(); +} + + +class DroneSafetyMonitorNode : public rclcpp::Node +{ +public: + rclcpp::Subscription::SharedPtr state_estimate_sub; + rclcpp::Publisher::SharedPtr state_estimate_timeout_pub; + rclcpp::TimerBase::SharedPtr timer; + float state_estimate_timeout; + + TimeChecker state_estimate_time_checker; + + DroneSafetyMonitorNode() : Node("odom_timeout_checker"){ + state_estimate_timeout = airstack::get_param(this, "state_estimate_timeout", 1.); + + // subscribers + state_estimate_sub = this->create_subscription("state_estimate", 10, + std::bind(&DroneSafetyMonitorNode::state_estimate_callback, + this, std::placeholders::_1)); + + // publishers + state_estimate_timeout_pub = this->create_publisher("state_estimate_timed_out", 10); + + // timers + timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(1000), + std::bind(&DroneSafetyMonitorNode::timer_callback, this)); + + } + + void state_estimate_callback(const nav_msgs::msg::Odometry::SharedPtr msg){ + state_estimate_time_checker.update(this->get_clock()->now()); + } + + void timer_callback(){ + std_msgs::msg::Bool state_estimate_timeout_msg; + state_estimate_timeout_msg.data = + state_estimate_time_checker.elapsed_since_last_update(this->get_clock()->now()) > state_estimate_timeout; + state_estimate_timeout_pub->publish(state_estimate_timeout_msg); + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml index dee935ec4..6301b0ea0 100644 --- a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml @@ -15,11 +15,20 @@ output="screen"> + + + + output="screen"> + + + + + + @@ -53,4 +62,12 @@ - \ No newline at end of file + + + + + + + diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt index 91bf2a3a0..68c4b7fb4 100644 --- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(mavros_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) find_package(airstack_common REQUIRED) +find_package(std_srvs REQUIRED) add_library(mavros_interface src/mavros_interface.cpp) target_compile_features(mavros_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 @@ -27,6 +28,7 @@ ament_target_dependencies( pluginlib tf2 airstack_common + std_srvs ) pluginlib_export_plugin_description_file(robot_interface plugins.xml) diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml index e7975ce03..bf85da151 100644 --- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml @@ -17,6 +17,7 @@ pluginlib tf2 airstack_common + std_srvs ament_lint_auto ament_lint_common diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py index a47e23547..e82cfa3f5 100755 --- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py @@ -3,9 +3,20 @@ import rclpy from rclpy.node import Node +from rclpy.time import Time +from rclpy.duration import Duration from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from rclpy.executors import MultiThreadedExecutor, Executor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from airstack_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry as NavOdometry +from geometry_msgs.msg import PoseStamped, TwistStamped +from scipy.spatial.transform import Rotation as R +import numpy as np +import tf2_ros +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import time class OdomModifier(Node): def __init__(self): @@ -16,19 +27,132 @@ def __init__(self): history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, depth=1) ''' - self.odom_subscriber = self.create_subscription(Odometry, "/" + os.getenv('ROBOT_NAME', "") + '/trajectory_controller/tracking_point', self.odom_callback, 1) + + self.declare_parameter('command_pose', True) + self.command_pose = self.get_parameter('command_pose').get_parameter_value().bool_value + + self.declare_parameter('max_velocity', 0.5) + self.max_velocity = self.get_parameter('max_velocity').get_parameter_value().double_value + + self.declare_parameter('target_frame', 'base_link') + self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.odom_subscriber = self.create_subscription(NavOdometry, "/" + os.getenv('ROBOT_NAME', "") + '/odometry_conversion/odometry', self.odom_callback, 1) + + self.callback_group = MutuallyExclusiveCallbackGroup() + self.tracking_point_subscriber = self.create_subscription(Odometry, "/" + os.getenv('ROBOT_NAME', "") + '/trajectory_controller/tracking_point', self.tracking_point_callback, 1, callback_group=self.callback_group) self.odom_publisher = self.create_publisher(PoseStamped, 'cmd_pose', 1) + self.vel_publisher = self.create_publisher(TwistStamped, 'cmd_velocity', 1) + + #self.odom_pos = [0., 0., 0.] + self.odom = None + + def get_yaw(self, q): + yaw = R.from_quat([q.x, q.y, q.z, q.w]) + yaw = yaw.as_euler('xyz') + return yaw[2] def odom_callback(self, msg): - out = PoseStamped() - out.header = msg.header - out.header.frame_id = 'base_link' - out.pose = msg.pose - self.odom_publisher.publish(out) + self.odom = msg + #self.odom_pos = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z] + #self.odom_yaw = self.get_yaw(msg.pose.pose.orientation) + + def get_tf(self, target_frame, source_frame, lookup_time, timeout=Duration(nanoseconds=0)): + exception = False + try: + t = self.tf_buffer.lookup_transform(target_frame, source_frame, lookup_time) + return t + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {target_frame} to {source_frame}: {ex}') + exception = True + + start_time = time.time() + if exception: + try: + t = self.tf_buffer.lookup_transform(target_frame, source_frame, lookup_time, timeout) + self.get_logger().info('elapsed %f' % (time.time() - start_time)) + return t + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Attempt 2: {target_frame} to {source_frame}: {ex}') + self.get_logger().info('elapsed %f' % (time.time() - start_time)) + return None + + def tracking_point_callback(self, msg): + if self.odom == None: + return + + if self.command_pose: + out = PoseStamped() + out.header = msg.header + out.header.frame_id = 'base_link' + out.pose = msg.pose + self.odom_publisher.publish(out) + else: + t = self.get_tf(self.target_frame, msg.header.frame_id, rclpy.time.Time(seconds=0, nanoseconds=0), + Duration(seconds=2, nanoseconds=0)) + if t == None: + return + + rot = R.from_quat([t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w]) + vel = np.array([msg.pose.position.x - self.odom.pose.pose.position.x, + msg.pose.position.y - self.odom.pose.pose.position.y, + msg.pose.position.z - self.odom.pose.pose.position.z]) + vel = rot.apply(vel) + mag = np.linalg.norm(vel) + if mag > self.max_velocity: + vel = vel/mag * self.max_velocity + + tracking_point_q = rot*R.from_quat([msg.pose.orientation.x, + msg.pose.orientation.y, + msg.pose.orientation.z, + msg.pose.orientation.w]) + tracking_point_yaw_tf = tracking_point_q.as_euler('xyz')[2] + tracking_point_yaw = self.get_yaw(msg.pose.orientation) + #self.get_logger().info('tp: ' + str(tracking_point_yaw) + ' ' + str(tracking_point_q.as_euler('xyz')[2])) + + odom_yaw = self.get_yaw(self.odom.pose.pose.orientation) + odom_t = self.get_tf(self.target_frame, self.odom.header.frame_id, self.odom.header.stamp) + if odom_t == None: + return + odom_rot = R.from_quat([odom_t.transform.rotation.x, + odom_t.transform.rotation.y, + odom_t.transform.rotation.z, + odom_t.transform.rotation.w]) + odom_q = R.from_quat([self.odom.pose.pose.orientation.x, + self.odom.pose.pose.orientation.y, + self.odom.pose.pose.orientation.z, + self.odom.pose.pose.orientation.w]) + odom_yaw_tf = (odom_rot*odom_q).as_euler('xyz')[2] + #self.get_logger().info('odom: ' + str(odom_yaw) + ' ' + str(odom_yaw_tf)) + #self.get_logger().info(str(odom_rot.as_euler('xyz')[2]) + ' ' + str(odom_q.as_euler('xyz')[2])) + + yawrate = np.arctan2(np.sin(tracking_point_yaw_tf - odom_yaw_tf), np.cos(tracking_point_yaw_tf - odom_yaw_tf)) + + out = TwistStamped() + out.header = msg.header + out.header.frame_id = self.target_frame + out.twist.linear.x = vel[0]#msg.pose.position.x - self.odom_pos[0] + out.twist.linear.y = vel[1]#msg.pose.position.y - self.odom_pos[1] + out.twist.linear.z = vel[2]#msg.pose.position.z - self.odom_pos[2] + out.twist.angular.z = yawrate + self.vel_publisher.publish(out) if __name__ == '__main__': rclpy.init(args=None) odom_modifier_node = OdomModifier() - rclpy.spin(odom_modifier_node) - odom_modifier_node.destroy_node() + + executor = MultiThreadedExecutor() + executor.add_node(odom_modifier_node) + executor.spin() + + #rclpy.spin(odom_modifier_node) + #odom_modifier_node.destroy_node() rclpy.shutdown() diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp index 5fb9f05d1..73fd9203a 100644 --- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace mavros_interface { @@ -31,7 +32,8 @@ namespace mavros_interface { class MAVROSInterface : public robot_interface::RobotInterface { private: // parameters - bool is_ardupilot; // TODO make this a launch file parameter + bool is_ardupilot; + float post_takeoff_command_delay_time; bool is_state_received_ = false; mavros_msgs::msg::State current_state_; @@ -45,10 +47,11 @@ class MAVROSInterface : public robot_interface::RobotInterface { rclcpp::CallbackGroup::SharedPtr service_callback_group; rclcpp::Client::SharedPtr set_mode_client_; rclcpp::Client::SharedPtr arming_client_; - rclcpp::Client::SharedPtr takeoff_client_; + rclcpp::Client::SharedPtr ardupilot_takeoff_client_; rclcpp::Publisher::SharedPtr attitude_target_pub_; rclcpp::Publisher::SharedPtr local_position_target_pub_; + rclcpp::Publisher::SharedPtr velocity_target_pub_; rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Subscription::SharedPtr mavros_odometry_sub_; @@ -57,6 +60,7 @@ class MAVROSInterface : public robot_interface::RobotInterface { MAVROSInterface() : RobotInterface("mavros_interface") { // params is_ardupilot = airstack::get_param(this, "is_ardupilot", false); + post_takeoff_command_delay_time = airstack::get_param(this, "post_takeoff_command_delay_time", 5.); // services service_callback_group = @@ -65,14 +69,16 @@ class MAVROSInterface : public robot_interface::RobotInterface { "mavros/set_mode", rmw_qos_profile_services_default, service_callback_group); arming_client_ = this->create_client( "mavros/cmd/arming", rmw_qos_profile_services_default, service_callback_group); - takeoff_client_ = this->create_client( - "mavros/cmd/takeoff", rmw_qos_profile_services_default, service_callback_group); + ardupilot_takeoff_client_ = this->create_client( + "ardupilot_takeoff", rmw_qos_profile_services_default, service_callback_group); // publishers attitude_target_pub_ = this->create_publisher( "mavros/setpoint_raw/attitude", 1); local_position_target_pub_ = this->create_publisher( "mavros/setpoint_position/local", 1); + velocity_target_pub_ = this->create_publisher( + "mavros/setpoint_raw/local", 1); // subscribers state_sub_ = this->create_subscription( @@ -86,6 +92,26 @@ class MAVROSInterface : public robot_interface::RobotInterface { // The MAVROS API only has two types of control: Attitude Control and // Position Control. + void velocity_callback(const geometry_msgs::msg::TwistStamped::SharedPtr cmd) { + if (!is_ardupilot || + (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > post_takeoff_command_delay_time))) { + mavros_msgs::msg::PositionTarget msg; + msg.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_BODY_NED; + msg.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX | + mavros_msgs::msg::PositionTarget::IGNORE_PY | + mavros_msgs::msg::PositionTarget::IGNORE_PZ | + mavros_msgs::msg::PositionTarget::IGNORE_AFX | + mavros_msgs::msg::PositionTarget::IGNORE_AFY | + mavros_msgs::msg::PositionTarget::IGNORE_AFZ | + mavros_msgs::msg::PositionTarget::IGNORE_YAW; + msg.velocity.x = cmd->twist.linear.x; + msg.velocity.y = cmd->twist.linear.y; + msg.velocity.z = cmd->twist.linear.z; + msg.yaw_rate = cmd->twist.angular.z; + velocity_target_pub_->publish(msg); + } + } + void attitude_thrust_callback(const mav_msgs::msg::AttitudeThrust::SharedPtr cmd) override { mavros_msgs::msg::AttitudeTarget mavros_cmd; mavros_cmd.header.stamp = this->get_clock()->now(); //.to_msg(); @@ -129,7 +155,7 @@ class MAVROSInterface : public robot_interface::RobotInterface { void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr cmd) override { if (!is_ardupilot || - (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > 5.))) { + (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > post_takeoff_command_delay_time))) { geometry_msgs::msg::PoseStamped cmd_copy = *cmd; local_position_target_pub_->publish(cmd_copy); } @@ -165,8 +191,6 @@ class MAVROSInterface : public robot_interface::RobotInterface { } bool disarm() override { - bool success = false; - auto request = std::make_shared(); request->value = false; @@ -187,14 +211,13 @@ class MAVROSInterface : public robot_interface::RobotInterface { bool takeoff() override { if (is_ardupilot) { - mavros_msgs::srv::CommandTOL::Request::SharedPtr takeoff_request = - std::make_shared(); - takeoff_request->altitude = 0.1; + std_srvs::srv::Trigger::Request::SharedPtr takeoff_request = + std::make_shared(); - std::cout << "ardupilot takeoff 1" << std::endl; - auto takeoff_result = takeoff_client_->async_send_request(takeoff_request); + std::cout << "calling ardupilot takeoff 1" << std::endl; + auto takeoff_result = ardupilot_takeoff_client_->async_send_request(takeoff_request); takeoff_result.wait(); - std::cout << "ardupilot takeoff 2" << std::endl; + std::cout << "calling ardupilot takeoff 2" << std::endl; if (takeoff_result.get()->success) { in_air = true; in_air_start_time = this->get_clock()->now(); diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt index 4e068f995..506ac7d5f 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt @@ -12,6 +12,8 @@ find_package(std_msgs REQUIRED) find_package(airstack_msgs REQUIRED) find_package(airstack_common REQUIRED) find_package(trajectory_library REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(std_srvs REQUIRED) add_executable(takeoff_landing_planner src/takeoff_landing_planner.cpp) target_include_directories(takeoff_landing_planner PUBLIC @@ -24,6 +26,8 @@ ament_target_dependencies( airstack_msgs airstack_common trajectory_library + mavros_msgs + std_srvs ) install(TARGETS takeoff_landing_planner diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml index 5b1118664..62c82f607 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml @@ -1,8 +1,9 @@ /**: ros__parameters: - takeoff_height: 2.0 - high_takeoff_height: 10.0 - takeoff_landing_velocity: 0.3 + takeoff_height: 5.0 + high_takeoff_height: 5.0 + takeoff_velocity: 1.0 + landing_velocity: 0.3 takeoff_acceptance_distance: 0.3 takeoff_acceptance_time: 10.0 landing_stationary_distance: 0.02 @@ -10,5 +11,5 @@ landing_tracking_point_ahead_time: 5.0 takeoff_path_roll: 0. # in degrees - takeoff_path_pitch: 20. # in degrees + takeoff_path_pitch: 0. # in degrees takeoff_path_relative_to_orientation: false diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp index f0d5edfde..3cd6bf7b8 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include #include #include @@ -14,7 +16,7 @@ class TakeoffLandingPlanner : public rclcpp::Node { private: // parameters - float takeoff_height, high_takeoff_height, takeoff_landing_velocity; + float takeoff_height, high_takeoff_height, takeoff_velocity, landing_velocity; float takeoff_acceptance_distance, takeoff_acceptance_time; float landing_stationary_distance, landing_acceptance_time; float landing_tracking_point_ahead_time; @@ -58,7 +60,10 @@ class TakeoffLandingPlanner : public rclcpp::Node { rclcpp::Publisher::SharedPtr landing_state_pub; // services + rclcpp::CallbackGroup::SharedPtr service_callback_group; rclcpp::Service::SharedPtr command_server; + rclcpp::Service::SharedPtr ardupilot_takeoff_server; + rclcpp::Client::SharedPtr takeoff_client; // timers rclcpp::TimerBase::SharedPtr timer; @@ -73,7 +78,9 @@ class TakeoffLandingPlanner : public rclcpp::Node { void set_takeoff_landing_command( const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response); - + void ardupilot_takeoff(const std::shared_ptr request, + std::shared_ptr response); + public: TakeoffLandingPlanner(); void timer_callback(); diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml index b3a2a699f..410fb2b74 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml @@ -14,6 +14,8 @@ airstack_msgs airstack_common trajectory_library + mavros_msgs + std_srvs ament_lint_auto ament_lint_common diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp index 1bc9d73cf..bdcc04683 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp @@ -4,7 +4,8 @@ TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_p // init parameters takeoff_height = airstack::get_param(this, "takeoff_height", 0.5); high_takeoff_height = airstack::get_param(this, "high_takeoff_height", 1.2); - takeoff_landing_velocity = airstack::get_param(this, "takeoff_landing_velocity", 0.3); + takeoff_velocity = airstack::get_param(this, "takeoff_velocity", 0.3); + landing_velocity = airstack::get_param(this, "landing_velocity", 0.3); takeoff_acceptance_distance = airstack::get_param(this, "takeoff_acceptance_distance", 0.1); takeoff_acceptance_time = airstack::get_param(this, "takeoff_acceptance_time", 2.0); landing_stationary_distance = airstack::get_param(this, "landing_stationary_distance", 0.02); @@ -43,11 +44,18 @@ TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_p landing_state_pub = this->create_publisher("landing_state", 1); // init services + service_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + takeoff_client = this->create_client("mavros/cmd/takeoff", rmw_qos_profile_services_default, + service_callback_group); command_server = this->create_service( "set_takeoff_landing_command", std::bind(&TakeoffLandingPlanner::set_takeoff_landing_command, this, std::placeholders::_1, std::placeholders::_2)); - + ardupilot_takeoff_server = this->create_service("ardupilot_takeoff", + std::bind(&TakeoffLandingPlanner::ardupilot_takeoff, this, + std::placeholders::_1, std::placeholders::_2)); + // init variables got_completion_percentage = false; is_tracking_point_received = false; @@ -58,13 +66,13 @@ TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_p // track_mode_srv.request.mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; high_takeoff = false; takeoff_traj_gen = - new TakeoffTrajectory(takeoff_height, takeoff_landing_velocity, takeoff_path_roll, + new TakeoffTrajectory(takeoff_height, takeoff_velocity, takeoff_path_roll, takeoff_path_pitch, takeoff_path_relative_to_orientation); high_takeoff_traj_gen = - new TakeoffTrajectory(high_takeoff_height, takeoff_landing_velocity, takeoff_path_roll, + new TakeoffTrajectory(high_takeoff_height, takeoff_velocity, takeoff_path_roll, takeoff_path_pitch, takeoff_path_relative_to_orientation); // TODO: this landing point is hardcoded. it should be parameterized - landing_traj_gen = new TakeoffTrajectory(-10000., takeoff_landing_velocity); + landing_traj_gen = new TakeoffTrajectory(-10000., landing_velocity); current_command = airstack_msgs::srv::TakeoffLandingCommand::Request::NONE; completion_percentage = 0.f; @@ -185,10 +193,10 @@ void TakeoffLandingPlanner::timer_callback() { "TransformException in TakeoffMonitor landing tf lookup: " << ex.what()); } bool tracking_point_check = - (z_distance / takeoff_landing_velocity) > landing_tracking_point_ahead_time; + (z_distance / landing_velocity) > landing_tracking_point_ahead_time; RCLCPP_INFO_STREAM( this->get_logger(), - "landing: " << z_distance << " " << (z_distance / takeoff_landing_velocity) << " " + "landing: " << z_distance << " " << (z_distance / landing_velocity) << " " << landing_tracking_point_ahead_time << " " << tracking_point_check); // ROS_INFO_STREAM("LANDING CHECK: " << time_diff << " " << time_check << " " << @@ -266,9 +274,26 @@ void TakeoffLandingPlanner::set_takeoff_landing_command( response->accepted = true; } + +void TakeoffLandingPlanner::ardupilot_takeoff(const std::shared_ptr request, + std::shared_ptr response){ + mavros_msgs::srv::CommandTOL::Request::SharedPtr takeoff_request = + std::make_shared(); + takeoff_request->altitude = takeoff_height; + + std::cout << "ardupilot takeoff 1" << std::endl; + auto takeoff_result = takeoff_client->async_send_request(takeoff_request); + takeoff_result.wait(); + std::cout << "ardupilot takeoff 2" << std::endl; + response->success = takeoff_result.get()->success; +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + std::shared_ptr node = std::make_shared(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py index 0675cc684..f724941da 100755 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py @@ -10,6 +10,7 @@ import rclpy from rclpy.node import Node +logger = None def get_velocities(traj, velocity, max_acc): v_prev = 0.0 @@ -287,12 +288,13 @@ def get_box_waypoints(attributes): # length, height): return traj -def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velocity): +def get_lawnmower_waypoints(attributes): # length, width, height, velocity): frame_id = str(attributes["frame_id"]) length = float(attributes["length"]) width = float(attributes["width"]) height = float(attributes["height"]) velocity = float(attributes["velocity"]) + vertical = bool(int(attributes["vertical"])) traj = TrajectoryXYZVYaw() traj.header.frame_id = frame_id @@ -326,7 +328,15 @@ def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velo wp2_.yaw = 0.0 wp2_.velocity = velocity + yaw = np.arctan2(wp2.position.y - wp1.position.y, wp2.position.z - wp1.position.z) + if i % 2 == 0: + if not vertical: + wp1.yaw = yaw + wp1_.yaw = yaw + wp2_.yaw = yaw + wp2.yaw = yaw + traj.waypoints.append(wp1) wp1_slow = copy.deepcopy(wp1_) wp1_slow.velocity = 0.1 @@ -335,6 +345,12 @@ def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velo traj.waypoints.append(wp2_) traj.waypoints.append(wp2) else: + if not vertical: + wp2.yaw = -yaw + wp2_.yaw = -yaw + wp1_.yaw = -yaw + wp1.yaw = -yaw + traj.waypoints.append(wp2) wp2_slow = copy.deepcopy(wp2_) wp2_slow.velocity = 0.1 @@ -350,6 +366,10 @@ def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velo wp.yaw = 0.0 wp.velocity = 0.5 traj.waypoints.append(wp) + + if not vertical: + for i, wp in enumerate(traj.waypoints): + wp.position.x, wp.position.y, wp.position.z = wp.position.z, wp.position.y, wp.position.x return traj @@ -439,6 +459,8 @@ def fixed_trajectory_callback(self, msg): trajectory_msg = get_line_waypoints(attributes) elif msg.type == "Point": trajectory_msg = get_point_waypoints(attributes) + elif msg.type == "Lawnmower": + trajectory_msg = get_lawnmower_waypoints(attributes) if trajectory_msg != None: self.trajectory_override_pub.publish(trajectory_msg) @@ -450,7 +472,8 @@ def fixed_trajectory_callback(self, msg): rclpy.init(args=None) node = FixedTrajectoryGenerator() - + logger = node.get_logger() + rclpy.spin(node) node.destroy_node() rclpy.shutdown() diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml index 2d00b94f5..bbdc7dbc4 100644 --- a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -22,6 +22,7 @@ + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml index 711ebb6ad..2085f01c9 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml @@ -23,6 +23,7 @@ + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp index b73402942..3d67045dc 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp @@ -34,6 +34,8 @@ class BehaviorExecutive : public rclcpp::Node { bt::Condition* disarm_commanded_condition; bt::Condition* takeoff_complete_condition; bt::Condition* landing_complete_condition; + bt::Condition* in_air_condition; + bt::Condition* state_estimate_timed_out_condition; std::vector conditions; // Action variables @@ -55,6 +57,7 @@ class BehaviorExecutive : public rclcpp::Node { rclcpp::Subscription::SharedPtr has_control_sub; rclcpp::Subscription::SharedPtr takeoff_state_sub; rclcpp::Subscription::SharedPtr landing_state_sub; + rclcpp::Subscription::SharedPtr state_estimate_timed_out_sub; // publishers @@ -76,6 +79,7 @@ class BehaviorExecutive : public rclcpp::Node { void has_control_callback(const std_msgs::msg::Bool::SharedPtr msg); void takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg); void landing_state_callback(const std_msgs::msg::String::SharedPtr msg); + void state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg); public: BehaviorExecutive(); diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp index e6bb9b049..68beaa030 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp @@ -17,6 +17,8 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { disarm_commanded_condition = new bt::Condition("Disarm Commanded", this); takeoff_complete_condition = new bt::Condition("Takeoff Complete", this); landing_complete_condition = new bt::Condition("Landing Complete", this); + in_air_condition = new bt::Condition("In Air", this); + state_estimate_timed_out_condition = new bt::Condition("State Estimate Timed Out", this); conditions.push_back(auto_takeoff_commanded_condition); conditions.push_back(takeoff_commanded_condition); conditions.push_back(armed_condition); @@ -32,6 +34,8 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { conditions.push_back(disarm_commanded_condition); conditions.push_back(takeoff_complete_condition); conditions.push_back(landing_complete_condition); + conditions.push_back(in_air_condition); + conditions.push_back(state_estimate_timed_out_condition); // actions arm_action = new bt::Action("Arm", this); @@ -70,6 +74,10 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { landing_state_sub = this->create_subscription("landing_state", 1, std::bind(&BehaviorExecutive::landing_state_callback, this, std::placeholders::_1)); + state_estimate_timed_out_sub = + this->create_subscription("state_estimate_timed_out", 1, + std::bind(&BehaviorExecutive::state_estimate_timed_out_callback, + this, std::placeholders::_1)); // publishers @@ -128,6 +136,7 @@ void BehaviorExecutive::timer_callback() { if (disarm_action->is_active()) { if (disarm_action->active_has_changed()) { + in_air_condition->set(false); airstack_msgs::srv::RobotCommand::Request::SharedPtr request = std::make_shared(); request->command = airstack_msgs::srv::RobotCommand::Request::DISARM; @@ -146,6 +155,7 @@ void BehaviorExecutive::timer_callback() { if (takeoff_action->is_active()) { // std::cout << "takeoff" << std::endl; takeoff_action->set_running(); + in_air_condition->set(true); if (takeoff_action->active_has_changed()) { // put trajectory controller in track mode airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = @@ -332,6 +342,10 @@ void BehaviorExecutive::landing_state_callback(const std_msgs::msg::String::Shar landing_state = msg->data; } +void BehaviorExecutive::state_estimate_timed_out_callback(const std_msgs::msg::Bool::SharedPtr msg){ + state_estimate_timed_out_condition->set(msg->data); +} + int main(int argc, char** argv) { rclcpp::init(argc, argv); std::shared_ptr node = std::make_shared(); diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree index cd527ed84..7d81c45fd 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree @@ -2,11 +2,15 @@ -> (Auto Takeoff Commanded) -> + + (State Estimate Timed Out) ? + (In Air) (Armed) (Offboard Mode) [Request Control] ? + (In Air) (Armed) [Arm] ? diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml index 64c1f54de..08f030326 100644 --- a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml @@ -36,3 +36,11 @@ trajectories: - x - y - height + - Lawnmower: + attributes: + - frame_id + - length + - width + - height + - velocity + - vertical diff --git a/robot/ros_ws/src/robot_bringup/config/core.perspective b/robot/ros_ws/src/robot_bringup/config/core.perspective index cb8bdb031..309cd73ca 100644 --- a/robot/ros_ws/src/robot_bringup/config/core.perspective +++ b/robot/ros_ws/src/robot_bringup/config/core.perspective @@ -4,14 +4,14 @@ "mainwindow": { "keys": { "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000008490000015c00000fc800000492000008490000018100000fc80000049200000000000000000a00000008490000018100000fc800000492')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000002800000015c000009ff000004920000028000000181000009ff0000049200000000000000000a000000028000000181000009ff00000492')", "type": "repr(QByteArray.hex)", - "pretty-print": " I \\ I I " + "pretty-print": " \\ " }, "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000002e8fc0100000002fc0000000000000780000003d200fffffffc0200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000014000000fe0000004a00fffffffc00000118000001e4000001e400fffffffc0100000002fb00000072007200710074005f006200650068006100760069006f0072005f0074007200650065005f0063006f006d006d0061006e0064005f005f004200650068006100760069006f007200540072006500650043006f006d006d0061006e00640050006c007500670069006e005f005f0031005f005f0100000000000005ba0000025d00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000005c0000001c00000016f00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000004b20000007e0000000000000000000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000002e8fc0100000002fc00000000000007800000044200fffffffc0200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000014000000fe0000004a00fffffffc00000118000001e4000001e400fffffffc0100000002fb00000072007200710074005f006200650068006100760069006f0072005f0074007200650065005f0063006f006d006d0061006e0064005f005f004200650068006100760069006f007200540072006500650043006f006d006d0061006e00640050006c007500670069006e005f005f0031005f005f0100000000000005ba000002cd00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000005c0000001c00000016f00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000004b20000007e0000000000000000000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", "type": "repr(QByteArray.hex)", - "pretty-print": " J rrqt_behavior_tree_command__BehaviorTreeCommandPlugin__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ 6MinimizedDockWidgetsToolbar " + "pretty-print": " B J rrqt_behavior_tree_command__BehaviorTreeCommandPlugin__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ 6MinimizedDockWidgetsToolbar " } }, "groups": { @@ -110,7 +110,7 @@ "plugin": { "keys": { "attribute_settings": { - "repr": "b'\\x80\\x04\\x95*\\x02\\x00\\x00\\x00\\x00\\x00\\x00\\x8c\\x0bcollections\\x94\\x8c\\x0bOrderedDict\\x94\\x93\\x94)R\\x94(\\x8c\\x07Figure8\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x012\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x012\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tRacetrack\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x06Circle\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x06radius\\x94\\x8c\\x012\\x94u\\x8c\\x04Line\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x012\\x94\\x8c\\x05width\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x05Point\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x01x\\x94\\x8c\\x010\\x94\\x8c\\x01y\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94uu.'", + "repr": "b'\\x80\\x04\\x95\\x97\\x02\\x00\\x00\\x00\\x00\\x00\\x00\\x8c\\x0bcollections\\x94\\x8c\\x0bOrderedDict\\x94\\x93\\x94)R\\x94(\\x8c\\x07Figure8\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x012\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x012\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tRacetrack\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x06Circle\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x06radius\\x94\\x8c\\x012\\x94u\\x8c\\x04Line\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x012\\x94\\x8c\\x05width\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x05Point\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x01x\\x94\\x8c\\x010\\x94\\x8c\\x01y\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tLawnmower\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x06length\\x94\\x8c\\x0210\\x94\\x8c\\x05width\\x94\\x8c\\x011\\x94\\x8c\\x06height\\x94\\x8c\\x0210\\x94\\x8c\\x08velocity\\x94\\x8c\\x011\\x94\\x8c\\x08vertical\\x94\\x8c\\x010\\x94uu.'", "type": "repr" }, "config_filename": { diff --git a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml index eff9dc542..8e16a0657 100644 --- a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml +++ b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml @@ -27,6 +27,8 @@ + + diff --git a/robot/ros_ws/src/robot_bringup/launch/standalone_sitl.launch b/robot/ros_ws/src/robot_bringup/launch/standalone_sitl.launch new file mode 100644 index 000000000..9beda4f24 --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/launch/standalone_sitl.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml index 0d5884445..ef426a07a 100644 --- a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml +++ b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml @@ -44,11 +44,33 @@ topics: from_domain: 2 to_domain: 1 + /robot_1/behavior/behavior_tree_graphviz: + type: std_msgs/msg/String + from_domain: 1 + to_domain: 0 + /robot_2/behavior/behavior_tree_graphviz: + type: std_msgs/msg/String + from_domain: 2 + to_domain: 0 /robot_3/behavior/behavior_tree_graphviz: type: std_msgs/msg/String from_domain: 3 to_domain: 0 + /robot_1/interface/mavros/global_position/raw/fix: + type: sensor_msgs/msg/NavSatFix + from_domain: 1 + to_domain: 0 + /robot_2/interface/mavros/global_position/raw/fix: + type: sensor_msgs/msg/NavSatFix + from_domain: 2 + to_domain: 0 + /robot_3/interface/mavros/global_position/raw/fix: + type: sensor_msgs/msg/NavSatFix + from_domain: 3 + to_domain: 0 + + # Bridge "/clock" topic from doman ID 2 to domain ID 3, # Override durability to be 'volatile' and override depth to be 1 clock: diff --git a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz index 6482cd95e..65d214896 100644 --- a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz +++ b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz @@ -4,16 +4,15 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /TF1/Frames1 + - /Sensors1 - /Perception1 - /Local1 - - /Local1/DROAN1 - - /Local1/DROAN1/Trimmed Global Plan for DROAN1 - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1 - - /Local1/Trajectory Controller1 - /Global1 Splitter Ratio: 0.590062141418457 - Tree Height: 701 + Tree Height: 570 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -67,7 +66,7 @@ Visualization Manager: base_link_stabilized: Value: false front_stereo: - Value: false + Value: true left_camera: Value: true look_ahead_point: @@ -85,7 +84,7 @@ Visualization Manager: odom_ned: Value: false ouster: - Value: false + Value: true right_camera: Value: true tracking_point: @@ -93,7 +92,7 @@ Visualization Manager: tracking_point_stabilized: Value: false world: - Value: false + Value: true Marker Scale: 1 Name: TF Show Arrows: true @@ -103,8 +102,6 @@ Visualization Manager: world: map_FLU: base_link_ground_truth: - {} - map: base_link: base_link_frd: {} @@ -115,6 +112,7 @@ Visualization Manager: {} ouster: {} + map: base_link_stabilized: {} look_ahead_point: @@ -210,7 +208,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: false + Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0 @@ -231,7 +229,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: odometry_conversion/odometry - Value: false + Value: true Enabled: true Name: Sensors - Class: rviz_common/Group @@ -463,7 +461,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: world + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -506,25 +504,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 17.393077850341797 + Distance: 14.042609214782715 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -2.118461847305298 - Y: 3.126241683959961 - Z: -0.541254997253418 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.3603954613208771 - Target Frame: + Pitch: 0.5953989028930664 + Target Frame: base_link Value: Orbit (rviz) - Yaw: 0.3381178677082062 + Yaw: 1.1703985929489136 Saved: ~ Window Geometry: Displays: @@ -533,10 +531,10 @@ Window Geometry: collapsed: false Front Left RGB: collapsed: false - Height: 1376 + Height: 1140 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -547,6 +545,6 @@ Window Geometry: collapsed: false Visual Odometry Features: collapsed: false - Width: 2490 - X: 1990 - Y: 27 + Width: 2190 + X: 245 + Y: 72 diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros index 3322151ca..de73353a5 100644 --- a/simulation/isaac-sim/docker/Dockerfile.isaac-ros +++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros @@ -23,6 +23,8 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \ gnupg2 \ unzip \ tmux \ + iputils-ping \ + net-tools \ && rm -rf /var/lib/apt/lists/* # setup keys @@ -77,6 +79,15 @@ RUN cd /tmp/ && \ colcon build --symlink-install && \ rm -rf /tmp/IsaacSim-${ISAAC_VERSION}.zip /tmp/${isaac_dir_name} +# Install mavlink-router +RUN apt update && apt install meson ninja-build +RUN git clone https://github.com/mavlink-router/mavlink-router.git --recurse /mavlink-router +WORKDIR /mavlink-router +RUN meson setup build . +RUN ninja -C build install +RUN rm -rf /mavlink-router +WORKDIR /isaac-sim + # copy over the AscentAeroSystemsSITLPackage COPY sitl_integration/AscentAeroSystemsSITLPackage /AscentAeroSystemsSITLPackage diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml index a238dd6e8..bd4c7b9e7 100644 --- a/simulation/isaac-sim/docker/docker-compose.yaml +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -58,6 +58,7 @@ services: # developer stuff - .dev:/root/.dev:rw # developer config - .bashrc:/root/.bashrc:rw # bash config + - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history # code - ../sitl_integration/kit-app-template/source/extensions/:/root/Documents/Kit/shared/exts/ - ../sitl_integration:/sitl_integration:rw diff --git a/simulation/isaac-sim/sitl_integration/control_test.py b/simulation/isaac-sim/sitl_integration/control_test.py new file mode 100755 index 000000000..f62f4f38f --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/control_test.py @@ -0,0 +1,180 @@ +#!/usr/bin/python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from mavros_msgs.srv import CommandBool, CommandTOL, SetMode +from mavros_msgs.msg import PositionTarget +import time + +POS = 1 +POS_RAW = 2 +POS_VEL_RAW = 3 + +class TimeValueSeries: + def __init__(self): + self.time_value_pairs = [] + + def add(self, time, value): + self.time_value_pairs.append((float(time), value)) + self.time_value_pairs = sorted(self.time_value_pairs, key=lambda x:x[0]) + + def get_value_at_time(self, desired_time): + dt = desired_time % self.time_value_pairs[-1][0] + for index, tv in enumerate(zip(reversed(self.time_value_pairs))): + time, value = tv[0] + if dt >= time: + return value + return None + + def get_times(self): + return list(map(lambda x:x[0], self.time_value_pairs)) + + def get_values(self): + return list(map(lambda x:x[1], self.time_value_pairs)) + + +class DroneControlNode(Node): + def __init__(self): + super().__init__('drone_control_node') + + # Get parameters + self.takeoff_wait_time = 10. + self.takeoff_height = 5. + self.setpoint_mode = POS_VEL_RAW + self.control_rate = 3. + self.do_plan = False + + self.x_plan = TimeValueSeries() + self.y_plan = TimeValueSeries() + + self.x_plan.add(0., 0.) + self.y_plan.add(0., 0.) + + self.x_plan.add(10., 10.) + self.y_plan.add(10., 0.) + + self.x_plan.add(20., 10.) + self.y_plan.add(20., 10.) + + self.x_plan.add(30., 0.) + self.y_plan.add(30., 10.) + + self.x_plan.add(40., 0.) + self.y_plan.add(40., 0.) + + # MAVROS service clients + self.set_mode_client = self.create_client(SetMode, 'mavros/set_mode') + self.arming_client = self.create_client(CommandBool, 'mavros/cmd/arming') + self.takeoff_client = self.create_client(CommandTOL, 'mavros/cmd/takeoff') + + # Publishers + if self.setpoint_mode == POS: + self.control_publisher = self.create_publisher(PoseStamped, 'mavros/setpoint_position/local', 10) + else: + self.control_publisher = self.create_publisher(PositionTarget, 'mavros/setpoint_raw/local', 10) + + # Start mission + self.set_guided_mode_and_takeoff() + + def call_service(self, client, request): + while not client.wait_for_service(timeout_sec=2.0): + self.get_logger().warn(f'Waiting for {client.srv_name} service...') + + future = client.call_async(request) + rclpy.spin_until_future_complete(self, future) + return future.result() + + def set_guided_mode_and_takeoff(self): + self.get_logger().info("Setting mode to GUIDED...") + mode_request = SetMode.Request() + mode_request.custom_mode = "GUIDED" + mode_response = self.call_service(self.set_mode_client, mode_request) + if not mode_response or not mode_response.mode_sent: + self.get_logger().error("Failed to set GUIDED mode") + return + + self.get_logger().info("Arming drone...") + arm_request = CommandBool.Request() + arm_request.value = True + arm_response = self.call_service(self.arming_client, arm_request) + if not arm_response or not arm_response.success: + self.get_logger().error("Failed to arm the drone") + return + + self.get_logger().info("Taking off...") + takeoff_request = CommandTOL.Request() + takeoff_request.altitude = self.takeoff_height + takeoff_response = self.call_service(self.takeoff_client, takeoff_request) + takeoff_skipped = False + if not takeoff_response or not takeoff_response.success: + self.get_logger().error("Failed to take off") + takeoff_skipped = True + + if not takeoff_skipped: + self.get_logger().info(f"Waiting {self.takeoff_wait_time} seconds before control...") + time.sleep(self.takeoff_wait_time) + + self.start_publishing_control() + + def start_publishing_control(self): + self.get_logger().info("Starting control publishing...") + rate = self.create_rate(self.control_rate) + + x = 10. + y = 0. + z = self.takeoff_height + start_time = time.time() + + while rclpy.ok(): + current_time = time.time() - start_time + + if self.do_plan: + x = self.x_plan.get_value_at_time(current_time) + y = self.y_plan.get_value_at_time(current_time) + print(x, y) + + if self.setpoint_mode == POS: + print('POS') + msg = PoseStamped() + msg.header.stamp = self.get_clock().now().to_msg() + msg.pose.position.x = x + msg.pose.position.y = y + msg.pose.position.z = z + msg.pose.orientation.w = 1.0 + elif self.setpoint_mode == POS_RAW: + print('POS_RAW') + msg = PositionTarget() + msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED + msg.type_mask = PositionTarget.IGNORE_VX | PositionTarget.IGNORE_VY | PositionTarget.IGNORE_VZ | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE + msg.position.x = x + msg.position.y = y + msg.position.z = z + msg.yaw = 0.0 + elif self.setpoint_mode == POS_VEL_RAW: + print('POS_VEL_RAW') + msg = PositionTarget() + msg.coordinate_frame = PositionTarget.FRAME_BODY_NED + msg.type_mask = PositionTarget.IGNORE_PX | PositionTarget.IGNORE_PY | PositionTarget.IGNORE_PZ | PositionTarget.IGNORE_AFX | PositionTarget.IGNORE_AFY | PositionTarget.IGNORE_AFZ | PositionTarget.IGNORE_YAW_RATE + msg.position.x = x + msg.position.y = y + msg.position.z = z + msg.velocity.x = 1.0 + msg.velocity.y = 0. + msg.velocity.z = 0. + msg.yaw = 0.0 + + self.control_publisher.publish(msg) + #rate.sleep() + time.sleep(1./self.control_rate) + + +def main(args=None): + rclpy.init(args=args) + node = DroneControlNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml index 5226507d8..b10730a8e 100644 --- a/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml @@ -7,15 +7,18 @@ windows: - cd /AscentAeroSystemsSITLPackage - gdb spirit_sitl -q --command=/sitl_integration/drag_and_drop/test.gdb -ex "r -S --model coaxial -I0 --base-port $BASE_PORT" - shell_command: + - sleep 2 - echo $ASCENT_SITL_PORT - echo $ISAAC_SIM_PORT - echo $AUTONOMY_STACK_PORT - - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + # - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + - mavlink-routerd -t 0 -p 127.0.0.1:$BASE_PORT -e 127.0.0.1:$ASCENT_SITL_PORT -e 127.0.0.1:$ISAAC_SIM_PORT -e 127.0.0.1:$AUTONOMY_STACK_PORT - shell_command: # - sleep 5 - export ROS_DOMAIN_ID=$DOMAIN_ID # - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros --ros-args -p use_sim_time:=true - > + while true; do ros2 run mavros mavros_node --ros-args -r __ns:=/$NAMESPACE/interface/mavros @@ -25,4 +28,5 @@ windows: -p fcu_protocol:=v2.0 --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_pluginlists.yaml --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml - -p use_sim_time:=true \ No newline at end of file + -p use_sim_time:=true 2>&1 | tee >(grep -m 1 "channel closed" && kill $$); + done \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/Makefile b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/Makefile new file mode 100644 index 000000000..bcb26a80b --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/Makefile @@ -0,0 +1,2 @@ +all: + g++ inject.cpp -fPIC -shared -o inject.so diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.cpp b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.cpp new file mode 100644 index 000000000..4a6fde5f7 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.cpp @@ -0,0 +1,167 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +unsigned int alarm(unsigned int seconds){ + return 0; +} + +bool enable_usleep = false; +int (*usleep_func)(useconds_t) = NULL; + +int usleep(useconds_t usec){ + //if (!usleep_func) + // usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + return 0; + //std::cout << "USLEEP " << usleep_counter << std::endl; + //usleep_counter++; + //return 0; + + //static int (*usleep_func)(useconds_t) = NULL; + if (!usleep_func) + usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + + if(enable_usleep && usec != 1000){ + //std::cout << "USLEEP: " << usec << std::endl; + usleep_func(usec); + } + + return 0; +} + +int nanosleep(const struct timespec *duration, + struct timespec * rem){ + std::cout << "NANOSLEEP 2" << std::endl; + return 0; +} + +int clock_nanosleep(clockid_t clockid, int flags, + const struct timespec *request, + struct timespec *remain) { + std::cout << "NANOSLEEP" << std::endl; + return 0; +} + + +int sockfd = -1; +struct sockaddr_in servaddr; + +static __attribute__((constructor)) void init(void){ + usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + /* + // Create socket + std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + //*/ +} + +void injected_function(unsigned long i){ + if(i < 41077729){//usleep_counter < usleep_limit) + printf("skpping\n"); + return; + } + else{ + //enable_usleep = true; + usleep_func(1001); + return; + } + + static auto start = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::high_resolution_clock::now() - start; + long long microseconds = std::chrono::duration_cast(elapsed).count(); + static long long prev_microseconds = microseconds; + static long long prev_i = i; + //std::cout << "injected function " << microseconds << " " << (microseconds - prev_microseconds) << " " << i << " " << (i - prev_i) << " " << (i - microseconds) << std::endl; + //std::cout << "size: " << sizeof(unsigned long) << std::endl; + prev_i = i; + prev_microseconds = microseconds; + + + if (sockfd == -1){ + // Create socket + //std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + //std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + //std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + //std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + } + + // Send message + //* + char message_type = 't'; + uint8_t message[16];//sizeof(message_type) + sizeof(i)]; + std::memcpy(&message[0], &message_type, sizeof(message_type)); + std::memcpy(&message[sizeof(i)], &i, sizeof(i)); + send(sockfd, message, sizeof(message), 0); + //send(sockfd, &i, sizeof(i), 0); + + // Receive response + char buffer[1024] = {0}; + int time_to_sleep; + //int valread = read(sockfd, buffer, 1024); + int valread = read(sockfd, &time_to_sleep, sizeof(time_to_sleep)); + //std::cout << "Server: " << buffer << std::endl; + //std::cout << "Server: " << time_to_sleep << std::endl; + if(time_to_sleep == 1000) + time_to_sleep = 1001; + if(time_to_sleep > 0) + if(usleep_func != NULL) + usleep_func(time_to_sleep); + //*/ +} + + +static __attribute__((destructor)) void end(void){ + close(sockfd); +} diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.so b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.so new file mode 100755 index 000000000..715ecaf94 Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/inject.so differ diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.sh b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.sh new file mode 100755 index 000000000..e467c9984 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.sh @@ -0,0 +1,20 @@ +#!/bin/bash + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +cd $SCRIPT_DIR/ + +#export BASE_PORT=5760 +#export ASCENT_SITL_PORT=14552 # port to talk to QGC +#export ISAAC_SIM_PORT=14553 # port for Isaac Sim +#export AUTONOMY_STACK_PORT=14554 # port for our autonomy stack + +export BASE_PORT=5760 +export ASCENT_SITL_PORT=14552 # port to talk to QGC +export ISAAC_SIM_PORT=14553 # port for Isaac Sim +export AUTONOMY_STACK_PORT=14554 # port for our autonomy stack +export MAVROS_LAUNCH_PORT=14555 +export SESSION_NAME=1 +export DOMAIN_ID=1 +export NAMESPACE=robot_1 + +tmuxp load standalone.yaml -s $SESSION_NAME diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml new file mode 100644 index 000000000..185a394c6 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml @@ -0,0 +1,27 @@ +session_name: Ascent Spirit +windows: + - window_name: SITL + layout: tiled + panes: + - shell_command: + - cd /AscentAeroSystemsSITLPackage + - gdb spirit_sitl -q --command=/sitl_integration/drag_and_drop/standalone/test.gdb -ex "r -S --model coaxial -I0 --base-port $BASE_PORT" + - shell_command: + - echo $ASCENT_SITL_PORT + - echo $ISAAC_SIM_PORT + - echo $AUTONOMY_STACK_PORT + - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + - shell_command: + - sleep 10 + - export ROS_DOMAIN_ID=$DOMAIN_ID + # - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros --ros-args -p use_sim_time:=true + - > + ros2 run mavros mavros_node + --ros-args + -r __ns:=/$NAMESPACE/interface/mavros + -p fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" + -p tgt_system:=1 + -p tgt_component:=1 + -p fcu_protocol:=v2.0 + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_pluginlists.yaml + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/test.gdb b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/test.gdb new file mode 100644 index 000000000..d9c532824 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/test.gdb @@ -0,0 +1,13 @@ +set environment LD_PRELOAD /sitl_integration/drag_and_drop/standalone/inject.so + +b _ZN7HALSITL9Scheduler10stop_clockEm +catch signal SIGSEGV + +commands 1 + silent + #info registers rsi + call (void)injected_function($rsi) + continue + +commands 2 + bt \ No newline at end of file