Add scan_filtered test and improve workpace tree

README.md

@@ -48,9 +48,9 @@ sudo apt-get install -y python3-pip ros-dev-tools stm32flash
 Create workspace folder and clone `rosbot_ros` repository:
 
 ```bash
-mkdir -p ros2_ws/src
+mkdir -p ros2_ws
 cd ros2_ws
-git clone https://github.com/husarion/rosbot_ros src/
+git clone https://github.com/husarion/rosbot_ros src/rosbot_ros
 ```
 
 ### Build and run on hardware
@@ -62,9 +62,9 @@ export HUSARION_ROS_BUILD=hardware
 
 source /opt/ros/$ROS_DISTRO/setup.bash
 
-vcs import src < src/rosbot/rosbot_hardware.repos
+vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos
 
-rm -r src/rosbot_gazebo
+rm -r src/rosbot_ros/rosbot_gazebo
 
 sudo rosdep init
 rosdep update --rosdistro $ROS_DISTRO
@@ -95,8 +95,8 @@ export HUSARION_ROS_BUILD=simulation
 
 source /opt/ros/$ROS_DISTRO/setup.bash
 
-vcs import src < src/rosbot/rosbot_hardware.repos
-vcs import src < src/rosbot/rosbot_simulation.repos
+vcs import src < src/rosbot_ros/rosbot/rosbot_hardware.repos
+vcs import src < src/rosbot_ros/rosbot/rosbot_simulation.repos
 
 # Build only imu_sensor_broadcaster from ros2_controllers
 cp -r src/ros2_controllers/imu_sensor_broadcaster src && rm -rf src/ros2_controllers
@@ -151,7 +151,7 @@ colcon test-result --verbose
 ### Format python code with [Black](https://github.com/psf/black)
 
 ```bash
-cd src/
+cd src/rosbot_ros
 black rosbot*
 ```
 

rosbot_bringup/test/test_utils.py

@@ -13,13 +13,15 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import math
+import random
 import time
 from threading import Event, Thread
 
 import rclpy
 from nav_msgs.msg import Odometry
 from rclpy.node import Node
-from sensor_msgs.msg import Imu, JointState
+from sensor_msgs.msg import Imu, JointState, LaserScan
 
 
 class BringupTestNode(Node):
@@ -33,13 +35,15 @@ def __init__(self, name="test_node", namespace=None):
         self.controller_odom_msg_event = Event()
         self.imu_msg_event = Event()
         self.ekf_odom_msg_event = Event()
+        self.scan_filter_event = Event()
 
         self.ros_spin_thread = None
         self.timer = None
 
     def create_test_subscribers_and_publishers(self):
         self.imu_pub = self.create_publisher(Imu, "/_imu/data_raw", 10)
         self.joint_pub = self.create_publisher(JointState, "/_motors_response", 10)
+        self.scan_pub = self.create_publisher(LaserScan, "scan", 10)
 
         self.joint_state_sub = self.create_subscription(
             JointState, "joint_states", self.joint_states_callback, 10
@@ -51,6 +55,9 @@ def create_test_subscribers_and_publishers(self):
         self.ekf_odom_sub = self.create_subscription(
             Odometry, "odometry/filtered", self.ekf_odometry_callback, 10
         )
+        self.scan_filtered_sub = self.create_subscription(
+            LaserScan, "scan_filtered", self.filtered_scan_callback, 10
+        )
 
     def start_node_thread(self):
         if not self.ros_spin_thread:
@@ -66,19 +73,24 @@ def start_publishing_fake_hardware(self):
 
     def timer_callback(self):
         self.publish_fake_hardware_messages()
+        self.publish_scan()
 
-    def joint_states_callback(self, data):
+    def joint_states_callback(self, msg: JointState):
         self.joint_state_msg_event.set()
 
-    def controller_odometry_callback(self, data):
+    def controller_odometry_callback(self, msg: Odometry):
         self.controller_odom_msg_event.set()
 
-    def imu_callback(self, data):
+    def imu_callback(self, msg: Imu):
         self.imu_msg_event.set()
 
-    def ekf_odometry_callback(self, data):
+    def ekf_odometry_callback(self, msg: Odometry):
         self.ekf_odom_msg_event.set()
 
+    def filtered_scan_callback(self, msg: LaserScan):
+        if len(msg.ranges) > 0:
+            self.scan_filter_event.set()
+
     def publish_fake_hardware_messages(self):
         imu_msg = Imu()
         imu_msg.header.stamp = self.get_clock().now().to_msg()
@@ -98,6 +110,21 @@ def publish_fake_hardware_messages(self):
         self.imu_pub.publish(imu_msg)
         self.joint_pub.publish(joint_state_msg)
 
+    def publish_scan(self):
+        msg = LaserScan()
+        msg.header.frame_id = "laser"
+        msg.angle_min = 0.0
+        msg.angle_max = 2.0 * math.pi
+        msg.angle_increment = 0.05
+        msg.time_increment = 0.1
+        msg.scan_time = 0.1
+        msg.range_min = 0.0
+        msg.range_max = 10.0
+
+        # fill ranges from 0.0m to 1.0m
+        msg.ranges = [random.random() for _ in range(int(msg.angle_max / msg.angle_increment))]
+        self.scan_pub.publish(msg)
+
 
 def wait_for_all_events(events, timeout):
     start_time = time.time()

rosbot_description/urdf/rosbot_macro.urdf.xacro

@@ -2,19 +2,8 @@
 <robot xmlns:xacro="http://wiki.ros.org/xacro">
   <xacro:macro name="rosbot_robot" params="mecanum use_sim simulation_engine namespace ">
 
-    <xacro:if value="${namespace == 'None'}">
-      <xacro:property name="namespace_ext" value="" />
-    </xacro:if>
-    <xacro:unless value="${namespace == 'None'}">
-        <xacro:property name="namespace_ext" value="${namespace}/" />
-    </xacro:unless>
-
-    <xacro:if value="${mecanum}">
-      <xacro:property name="wheel_radius" value="0.0470" />
-    </xacro:if>
-    <xacro:unless value="${mecanum}">
-      <xacro:property name="wheel_radius" value="0.0425" />
-    </xacro:unless>
+    <xacro:property name="ns" value="${namespace + '/' if namespace != 'None' else ''}" />
+    <xacro:property name="wheel_radius" value="${0.0470 if mecanum else 0.0425}" />
 
     <!-- INCLUDE ROBOT PARTS DEFINITIONS -->
     <xacro:include filename="$(find rosbot_description)/urdf/body.urdf.xacro" ns="body" />
@@ -31,13 +20,13 @@
 
     <!-- ROS2 CONTROL -->
     <xacro:unless value="$(arg use_sim)">
-      <ros2_control name="${namespace_ext}imu" type="sensor">
+      <ros2_control name="${ns}imu" type="sensor">
         <hardware>
           <plugin>rosbot_hardware_interfaces/RosbotImuSensor</plugin>
           <param name="connection_timeout_ms">120000</param>
           <param name="connection_check_period_ms">500</param>
         </hardware>
-        <sensor name="${namespace_ext}imu">
+        <sensor name="${ns}imu">
           <state_interface name="orientation.x" />
           <state_interface name="orientation.y" />
           <state_interface name="orientation.z" />
@@ -52,7 +41,7 @@
       </ros2_control>
     </xacro:unless>
 
-    <ros2_control name="${namespace_ext}wheels" type="system">
+    <ros2_control name="${ns}wheels" type="system">
       <hardware>
         <xacro:unless value="$(arg use_sim)">
           <plugin>rosbot_hardware_interfaces/RosbotSystem</plugin>
@@ -101,7 +90,7 @@
 
       <xacro:if value="${use_sim}">
         <xacro:if value="${simulation_engine == 'ignition-gazebo'}">
-          <sensor name="${namespace_ext}imu">
+          <sensor name="${ns}imu">
             <state_interface name="orientation.x" />
             <state_interface name="orientation.y" />
             <state_interface name="orientation.z" />
@@ -140,16 +129,16 @@
           </plugin>
         </gazebo>
         <gazebo reference="imu_link">
-          <sensor name="${namespace_ext}imu" type="imu">
+          <sensor name="${ns}imu" type="imu">
             <always_on>true</always_on>
             <update_rate>25</update_rate>
-            <topic>${namespace_ext}imu/data_raw</topic>
+            <topic>${ns}imu/data_raw</topic>
             <visualize>false</visualize>
             <enable_metrics>false</enable_metrics>
             <frame_id>imu_link</frame_id>
             <ignition_frame_id>imu_link</ignition_frame_id>
             <ros>
-              <namespace>${namespace_ext}</namespace>
+              <namespace>${namespace}</namespace>
             </ros>
           </sensor>
         </gazebo>

rosbot_gazebo/test/test_utils.py

@@ -60,7 +60,7 @@ def __init__(self, name="test_node", namespace=None):
         for range_topic_name in self.RANGE_SENSORS_TOPICS:
             sub = self.create_subscription(LaserScan, range_topic_name, self.ranges_callback, 10)
             self.range_subs.append(sub)
-        self.scan_sub = self.create_subscription(LaserScan, "scan", self.scan_callback, 10)
+        self.scan_sub = self.create_subscription(LaserScan, "scan_filtered", self.scan_callback, 10)
 
         # Timer - send cmd_vel and check if the time needed for speed stabilization has elapsed
         self.timer = self.create_timer(0.1, self.timer_callback)
@@ -128,26 +128,26 @@ def is_initialized(self):
             self.robot_initialized_event.set()
         return self.robot_initialized_event.is_set()
 
-    def controller_callback(self, data: Odometry):
+    def controller_callback(self, msg: Odometry):
         if not self.is_controller_msg:
             self.get_logger().info("Controller message arrived")
         self.is_controller_msg = True
-        self.controller_twist = data.twist.twist
-        self.is_controller_odom_correct = self.is_twist_ok(data.twist.twist)
+        self.controller_twist = msg.twist.twist
+        self.is_controller_odom_correct = self.is_twist_ok(msg.twist.twist)
 
-    def ekf_callback(self, data: Odometry):
+    def ekf_callback(self, msg: Odometry):
         if not self.is_ekf_msg:
             self.get_logger().info("EKF message arrived")
         self.is_ekf_msg = True
-        self.ekf_twist = data.twist.twist
-        self.is_ekf_odom_correct = self.is_twist_ok(data.twist.twist)
+        self.ekf_twist = msg.twist.twist
+        self.is_ekf_odom_correct = self.is_twist_ok(msg.twist.twist)
 
-    def imu_callback(self, data):
+    def imu_callback(self, msg: Imu):
         if not self.is_imu_msg:
             self.get_logger().info("IMU message arrived")
         self.is_imu_msg = True
 
-    def joint_states_callback(self, data):
+    def joint_states_callback(self, msg: JointState):
         if not self.is_joint_msg:
             self.get_logger().info("Joint State message arrived")
         self.is_joint_msg = True
@@ -164,22 +164,22 @@ def timer_callback(self):
                 )
                 self.vel_stabilization_time_event.set()
 
-    def scan_callback(self, data: LaserScan):
-        self.get_logger().debug(f"Received scan length: {len(data.ranges)}")
-        if data.ranges:
+    def scan_callback(self, msg: LaserScan):
+        self.get_logger().debug(f"Received scan length: {len(msg.ranges)}")
+        if msg.ranges:
             self.scan_event.set()
 
-    def ranges_callback(self, data: LaserScan):
-        index = self.RANGE_SENSORS_FRAMES.index(data.header.frame_id)
-        if len(data.ranges) == 1:
+    def ranges_callback(self, msg: LaserScan):
+        index = self.RANGE_SENSORS_FRAMES.index(msg.header.frame_id)
+        if len(msg.ranges) == 1:
             self.ranges_events[index].set()
 
-    def camera_image_callback(self, data: Image):
-        if data.data:
+    def camera_image_callback(self, msg: Image):
+        if msg.data:
             self.camera_color_event.set()
 
-    def camera_points_callback(self, data: PointCloud2):
-        if data.data:
+    def camera_points_callback(self, msg: PointCloud2):
+        if msg.data:
             self.camera_points_event.set()
 
     def publish_cmd_vel_msg(self):