diff --git a/CMakeLists.txt b/CMakeLists.txt index 9cb6c16d..243074f6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,10 +17,12 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rclpy REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -684,6 +686,13 @@ ENDIF(rviz_default_plugins_FOUND) # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} #) +# Install Python executables +install(PROGRAMS + scripts/transform_to_tf.py + scripts/yaml_to_camera_info.py + DESTINATION lib/${PROJECT_NAME} +) + ## Mark executables and/or libraries for installation install(TARGETS rtabmap_sync @@ -747,6 +756,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ #) install(DIRECTORY launch/ros2/. + launch/calibration # launch/data # launch/demo DESTINATION share/${PROJECT_NAME}/launch diff --git a/launch/ros2/euroc_datasets.launch.py b/launch/ros2/euroc_datasets.launch.py new file mode 100644 index 00000000..f3f1ece0 --- /dev/null +++ b/launch/ros2/euroc_datasets.launch.py @@ -0,0 +1,146 @@ + +# Example to run euroc datasets: +# $ sudo pip install rosbags # See https://docs.openvins.com/dev-ros1-to-ros2.html +# $ rosbags-convert V1_01_easy.bag +# $ rosbags-convert MH_01_easy.bag +# +# $ ros2 launch rtabmap_ros euroc_datasets.launch.py gt:=true +# $ cd V1_01_easy +# $ ros2 bag play V1_01_easy.db3 --clock +# +# $ ros2 launch rtabmap_ros euroc_datasets.launch.py gt:=false +# $ cd MH_01_easy +# $ ros2 bag play MH_01_easy.db3 --clock + + +from launch import LaunchDescription +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.conditions import IfCondition, UnlessCondition +from launch_ros.substitutions import FindPackageShare +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.actions import SetParameter + +def generate_launch_description(): + + ground_truth = LaunchConfiguration('gt') + + parameters={ + 'frame_id':'base_link', + 'subscribe_stereo':True, + 'subscribe_odom_info':True, + 'wait_imu_to_init':True, + 'approx_sync':False, + # RTAB-Map's parameters should all be string type: + 'RGBD/CreateOccupancyGrid':'false', + 'Rtabmap/CreateIntermediateNodes':'true', + 'RGBD/LinearUpdate':'0', + 'RGBD/AngularUpdate':'0'} + + remappings=[ + ('left/image_rect', '/stereo_camera/left/image_rect'), + ('left/camera_info', '/stereo_camera/left/camera_info'), + ('right/image_rect', '/stereo_camera/right/image_rect'), + ('right/camera_info', '/stereo_camera/right/camera_info'), + ('imu', '/imu/data')] + + + return LaunchDescription([ + + DeclareLaunchArgument( + 'gt', default_value='false', + description='If the VH rosbag sequence is used, you can enable ground truth.'), + + SetParameter(name='use_sim_time', value=True), + # 'use_sim_time' will be set on all nodes following the line above + + # Nodes to launch + Node( + package='rtabmap_ros', executable='stereo_odometry', output='screen', + parameters=[parameters], + remappings=remappings), + + Node( + condition=IfCondition(ground_truth), + package='rtabmap_ros', executable='rtabmap', output='screen', + parameters=[parameters, + { 'ground_truth_frame_id':'world', + 'ground_truth_base_frame_id':'base_link_gt'}], + remappings=remappings, + arguments=['-d']), + Node( + condition=UnlessCondition(ground_truth), + package='rtabmap_ros', executable='rtabmap', output='screen', + parameters=[parameters], + remappings=remappings, + arguments=['-d']), + + Node( + package='rtabmap_ros', executable='rtabmapviz', output='screen', + parameters=[parameters], + remappings=remappings), + + # Image rectification and publishing synchronized camera_info + Node( + package='rtabmap_ros', executable='yaml_to_camera_info.py', output='screen', + parameters=[{'yaml_path': [FindPackageShare('rtabmap_ros'), '/launch/calibration/euroc_left.yaml']}], + remappings=[ + ('image', '/cam0/image_raw'), + ('camera_info', 'left/camera_info')], + namespace='stereo_camera'), + + Node( + package='rtabmap_ros', executable='yaml_to_camera_info.py', output='screen', + parameters=[{'yaml_path': [FindPackageShare('rtabmap_ros'), '/launch/calibration/euroc_right.yaml']}], + remappings=[ + ('image', '/cam1/image_raw'), + ('camera_info', 'right/camera_info')], + namespace='stereo_camera'), + + Node( + package='image_proc', executable='image_proc', output='screen', + remappings=[ + ('image_raw', '/cam0/image_raw'), + ('image', '/cam0/image_raw')], + namespace='stereo_camera/left'), + Node( + package='image_proc', executable='image_proc', output='screen', + remappings=[ + ('image_raw', '/cam1/image_raw'), + ('image', '/cam1/image_raw')], + namespace='stereo_camera/right'), + + Node( + package='imu_complementary_filter', executable='complementary_filter_node', output='screen', + parameters=[{'use_mag': False, 'world_frame':'enu', 'publish_tf':False}], + remappings=[('imu/data_raw', '/imu0')]), + + # create a fake tf tree + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['0', '0', '0', '3.1415926', '-1.570796', '0', 'base_link', 'imu4']), + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['-0.021640', '-0.064677', '0.009811', '1.555925', '0.025777', '0.003757', 'imu4', 'cam0']), + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['-0.019844', '0.045369', '0.007862', '1.558237', '0.025393', '0.017907', 'imu4', 'cam1']), + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['0.12395', '-0.02781', '-0.06901', '0', '0', '0', 'vicon/firefly_sbx/firefly_sbx', 'base_link_gt']), + Node( + package='tf2_ros', executable='static_transform_publisher', output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'world', 'map']), + + Node( + package='rtabmap_ros', executable='transform_to_tf.py', output='screen', + parameters=[{'frame_id': 'world', 'child_frame_id': 'vicon/firefly_sbx/firefly_sbx'}], + remappings=[('transform', '/vicon/firefly_sbx/firefly_sbx')]), + ]) + + diff --git a/launch/ros2/rgbdslam_datasets.launch.py b/launch/ros2/rgbdslam_datasets.launch.py index 5ecaad9c..4adb1400 100644 --- a/launch/ros2/rgbdslam_datasets.launch.py +++ b/launch/ros2/rgbdslam_datasets.launch.py @@ -13,13 +13,14 @@ # $ ros2 launch rtabmap_ros rgbdslam_datasets.launch.py # $ cd rgbd_dataset_freiburg3_long_office_household_frameid_fixed -# $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 +# $ ros2 bag play rgbd_dataset_freiburg3_long_office_household_frameid_fixed.db3 --clock from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.actions import SetParameter def generate_launch_description(): @@ -45,6 +46,9 @@ def generate_launch_description(): return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), + # 'use_sim_time' will be set on all nodes following the line above + # Nodes to launch Node( package='rtabmap_ros', executable='rgbd_odometry', output='screen', @@ -62,8 +66,8 @@ def generate_launch_description(): parameters=parameters, remappings=remappings), - # /tf topic is not recognized in ROS2, create a fake tf + # /tf topic is missing in the converted ROS2 bag, create a fake tf Node( package='tf2_ros', executable='static_transform_publisher', output='screen', - arguments=['0.0', '0.0', '0.0', '-1.57', '0.0', '-1.57', 'kinect', 'openni_rgb_optical_frame']), + arguments=['0.0', '0.0', '0.0', '-1.57079632679', '0.0', '-1.57079632679', 'kinect', 'openni_rgb_optical_frame']), ]) diff --git a/package.xml b/package.xml index 88b0d110..0bd552ea 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ https://github.com/introlab/rtabmap_ros ament_cmake + ament_cmake_python ament_lint_auto ament_lint_common diff --git a/scripts/transform_to_tf.py b/scripts/transform_to_tf.py index f1756aba..fe393fbd 100755 --- a/scripts/transform_to_tf.py +++ b/scripts/transform_to_tf.py @@ -1,32 +1,44 @@ -#!/usr/bin/env python -import rospy -import tf +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node from geometry_msgs.msg import TransformStamped +from tf2_ros import TransformBroadcaster -def callback(transform): - global br - global frame_id - global child_frame_id - local_frame_id = transform.header.frame_id - local_child_frame_id = transform.child_frame_id - if not local_frame_id: - local_frame_id = frame_id - if not local_child_frame_id: - local_child_frame_id = child_frame_id - br.sendTransform( - (transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z), - (transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w), - transform.header.stamp, - child_frame_id, - frame_id) +class TransformToTf(Node): + + def __init__(self): + super().__init__('transform_to_tf') + + self.declare_parameter('frame_id', 'world') + self.declare_parameter('child_frame_id', 'transform') + self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value + self.child_frame_id = self.get_parameter('child_frame_id').get_parameter_value().string_value + + self.tf_broadcaster = TransformBroadcaster(self) + + self.subscription = self.create_subscription( + TransformStamped, + 'transform', + self.callback, + 1) + self.subscription # prevent unused variable warning + + def callback(self, t): + if not t.header.frame_id: + t.header.frame_id = self.frame_id + if not t.child_frame_id: + t.child_frame_id = self.child_frame_id + + self.tf_broadcaster.sendTransform(t) + + +def main(args=None): + rclpy.init(args=args) + transform_to_tf = TransformToTf() + rclpy.spin(transform_to_tf) + transform_to_tf.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - - rospy.init_node("transform_to_tf", anonymous=True) - - frame_id = rospy.get_param('~frame_id', 'world') - child_frame_id = rospy.get_param('~child_frame_id', 'transform') - - br = tf.TransformBroadcaster() - rospy.Subscriber("transform", TransformStamped, callback, queue_size=1) - rospy.spin() + main() + diff --git a/scripts/yaml_to_camera_info.py b/scripts/yaml_to_camera_info.py index 77132ab3..7d117634 100755 --- a/scripts/yaml_to_camera_info.py +++ b/scripts/yaml_to_camera_info.py @@ -1,6 +1,7 @@ -#!/usr/bin/env python -import rospy +#!/usr/bin/env python3 +import rclpy import yaml +from rclpy.node import Node from sensor_msgs.msg import CameraInfo from sensor_msgs.msg import Image @@ -8,37 +9,54 @@ def yaml_to_CameraInfo(yaml_fname): with open(yaml_fname, "r") as file_handle: calib_data = yaml.load(file_handle) - camera_info_msg = CameraInfo() - camera_info_msg.width = calib_data["image_width"] - camera_info_msg.height = calib_data["image_height"] - camera_info_msg.K = calib_data["camera_matrix"]["data"] - camera_info_msg.D = calib_data["distortion_coefficients"]["data"] - camera_info_msg.R = calib_data["rectification_matrix"]["data"] - camera_info_msg.P = calib_data["projection_matrix"]["data"] - camera_info_msg.distortion_model = calib_data["distortion_model"] - return camera_info_msg + msg = CameraInfo() + msg.width = calib_data["image_width"] + msg.height = calib_data["image_height"] + msg.k = calib_data["camera_matrix"]["data"] + msg.d = calib_data["distortion_coefficients"]["data"] + msg.r = calib_data["rectification_matrix"]["data"] + msg.p = calib_data["projection_matrix"]["data"] + msg.distortion_model = calib_data["distortion_model"] + return msg -def callback(image): - global publisher - global camera_info_msg - global frameId - camera_info_msg.header = image.header - if frameId: - camera_info_msg.header.frame_id = frameId - publisher.publish(camera_info_msg) +class YamlToCameraInfo(Node): + + def __init__(self): + super().__init__('yaml_to_camera_info') + + self.declare_parameter('yaml_path', '') + yaml_path = self.get_parameter('yaml_path').get_parameter_value().string_value + + if not yaml_path: + print('yaml_path parameter should be set to path of the calibration file!') + sys.exit(1) + + self.declare_parameter('frame_id', '') + self.frame_id = self.get_parameter('frame_id').get_parameter_value().string_value + + self.camera_info_msg = yaml_to_CameraInfo(yaml_path) + + self.publisher_ = self.create_publisher(CameraInfo, 'camera_info', 1) + self.subscription = self.create_subscription( + Image, + 'image', + self.callback, + 1) + self.subscription # prevent unused variable warning + + def callback(self, image): + self.camera_info_msg.header = image.header + if self.frame_id: + self.camera_info_msg.header.frame_id = self.frame_id + self.publisher_.publish(self.camera_info_msg) + + +def main(args=None): + rclpy.init(args=args) + yaml_to_camera_info = YamlToCameraInfo() + rclpy.spin(yaml_to_camera_info) + yaml_to_camera_info.destroy_node() + rclpy.shutdown() if __name__ == "__main__": - - rospy.init_node("yaml_to_camera_info", anonymous=True) - - yaml_path = rospy.get_param('~yaml_path', '') - if not yaml_path: - print('yaml_path parameter should be set to path of the calibration file!') - sys.exit(1) - - frameId = rospy.get_param('~frame_id', '') - camera_info_msg = yaml_to_CameraInfo(yaml_path) - - publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=1) - rospy.Subscriber("image", Image, callback, queue_size=1) - rospy.spin() + main() diff --git a/src/MsgConversion.cpp b/src/MsgConversion.cpp index 04ca37ee..de3d525a 100644 --- a/src/MsgConversion.cpp +++ b/src/MsgConversion.cpp @@ -63,7 +63,7 @@ void transformToTF(const rtabmap::Transform & transform, tf2::Transform & tfTran } else { - tfTransform = tf2::Transform(); + tfTransform = tf2::Transform(tf2::Quaternion(0,0,0,0)); } } @@ -91,6 +91,7 @@ void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs: else { msg = geometry_msgs::msg::Transform(); + msg.rotation.w = 0; // null } } @@ -119,6 +120,7 @@ void transformToPoseMsg(const rtabmap::Transform & transform, geometry_msgs::msg else { msg = geometry_msgs::msg::Pose(); + msg.orientation.w = 0; // null } }