update rtabmap

luxonis · Jan 8, 2024 · 1c864f6 · 1c864f6
1 parent b21131e
commit 1c864f6
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 16 deletions.
diff --git a/rae_hw/scripts/mock_wheels.py b/rae_hw/scripts/mock_wheels.py
@@ -4,20 +4,28 @@
 
 from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
-
+from tf2_ros import TransformBroadcaster, TransformStamped
 
 class MockWheels(Node):
     def __init__(self):
         super().__init__('mock_wheels')
-        self._odom_pub = self.create_publisher(Odometry, 'odom', 10)
+        self._odom_pub = self.create_publisher(Odometry, 'diff_controller/odom', 10)
+        self._tf_broadcaster = TransformBroadcaster(self)
         self._cmd_vel_sub = self.create_subscription(Twist, 'cmd_vel', self.cmd_vel_callback, 10)
-        self.timer = self.create_timer(1, self.timer_callback)
+        self.timer = self.create_timer(100, self.timer_callback)
 
     def cmd_vel_callback(self, msg: Twist):
         self.get_logger().info(f'I heard: {msg.angular.x}, {msg.angular.z}')
 
     def timer_callback(self):
         msg = Odometry()
+        tf = TransformStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "odom"
+        tf.header.stamp = self.get_clock().now().to_msg()
+        tf.header.frame_id = "odom"
+        tf.child_frame_id = "base_footprint"
+        self._tf_broadcaster.sendTransform(tf)
         self._odom_pub.publish(msg)
 
 if __name__ == '__main__':

diff --git a/robot_py/robot_py/robot/api/ros/ros_interface.py b/robot_py/robot_py/robot/api/ros/ros_interface.py
@@ -173,8 +173,9 @@ def _spin(self):
 
     def stop_ros_process(self):
         """Stop the ROS2 hardware process by terminating the related subprocess."""
-        self._stop_event.set()
-        self._process.join()
+        if self._launch_service:
+            self._stop_event.set()
+            self._process.join()
 
     def stop(self) -> None:
         """

diff --git a/robot_py/robot_py/robot/perception/perception_system.py b/robot_py/robot_py/robot/perception/perception_system.py
@@ -56,6 +56,7 @@ def __init__(self, namespace=''):
         Args:
         ----
             namespace (str, optional): The namespace for the ROS nodes. Defaults to ''.
+
         """
         self._namespace = namespace
         self._pipeline = None
@@ -160,6 +161,7 @@ def add_ros_imu_stream(self, stream_name, topic_name, frame_name):
         Args:
         ----
             stream_name (str): The name of the ROS stream to be added.
+            topic_name (str): The name of the ROS topic to be published to.
             frame_name (str): The name of the ROS frame.
 
         """
@@ -325,12 +327,8 @@ def setup_rtabmap(self):
         self.start_pipeline(rtabmap_pipeline())
 
         self.add_ros_imu_stream("imu", "imu/data", "rae_imu_frame")
-
         self.add_ros_img_stream(
-            "left", "left/image_rect", "rae_left_camera_optical_frame", dai.CameraBoardSocket.CAM_B, 640, 400)
-
-        self.add_ros_img_stream(
-            "right", "right/image_rect", "rae_right_camera_optical_frame", dai.CameraBoardSocket.CAM_C, 640, 400)
+            "right", "right/image_raw", "rae_right_camera_optical_frame", dai.CameraBoardSocket.CAM_C, 640, 400)
 
         self.add_ros_img_stream("stereo", "stereo/image_raw",
                                 "rae_right_camera_optical_frame", dai.CameraBoardSocket.CAM_C, 640, 400)
@@ -349,9 +347,13 @@ def setup_rtabmap(self):
         self.add_composable_node(
             'laserscan_kinect', 'laserscan_kinect::LaserScanKinectNode', self.scan_front_opts)
 
+        self.opts_rectify = dai_ros.ROSNodeOptions("RectifyNode", self._namespace, self._config_path, {
+                                                   "image": "right/image_raw", "camera_info": "right/camera_info", "image_rect": "right/image_rect"})
 
+        self.add_composable_node(
+            'depthai_filters', 'depthai_filters::Rectify', self.opts_rectify)
         self.opts_rtabmap = dai_ros.ROSNodeOptions("rtabmap", self._namespace, self._config_path,
-                                                   {
+                                                   {"odom": "diff_controller/odom",
                                                     "rgb/image": "right/image_rect",
                                                     "rgb/camera_info": "right/camera_info",
                                                     "depth/image": "stereo/image_raw"})

diff --git a/robot_py/robot_py/robot/perception/pipeline.py b/robot_py/robot_py/robot/perception/pipeline.py
@@ -101,6 +101,8 @@ def rtabmap_pipeline():
         right.setFps(30)
         right.setVideoSize(640, 400)
         right.setInterleaved(False)
+        right.initialControl.setMisc('stride-align', 1)
+        right.initialControl.setMisc('scanline-align', 1)
         stereo = pipeline.create(dai.node.StereoDepth)
         left.video.link(stereo.left)
         right.video.link(stereo.right)
@@ -110,14 +112,10 @@ def rtabmap_pipeline():
         xout_stereo.input.setBlocking(False)
         stereo.depth.link(xout_stereo.input)
 
-        xout_left = pipeline.create(dai.node.XLinkOut)
-        xout_left.setStreamName("left")
-        xout_left.input.setBlocking(False)
-        stereo.rectifiedLeft.link(xout_left.input)
         xout_right = pipeline.create(dai.node.XLinkOut)
         xout_right.setStreamName("right")
         xout_right.input.setBlocking(False)
-        stereo.rectifiedRight.link(xout_right.input)
+        right.video.link(xout_right.input)
         return pipeline
 
 def sai_pipeline():