feat: improved table detection node

.gitignore

@@ -0,0 +1 @@
+__pycache__/

localization_server/config/amcl_config.yaml

@@ -6,7 +6,7 @@ amcl:
     alpha3: 0.2
     alpha4: 0.2
     alpha5: 0.2
-    base_frame_id: "robot_base_footprint"
+    base_frame_id: "robot_base_link"
     beam_skip_distance: 0.5
     beam_skip_error_threshold: 0.9
     beam_skip_threshold: 0.3
@@ -15,7 +15,7 @@ amcl:
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
     laser_max_range: 100.0
-    laser_min_range: -1.0
+    laser_min_range: 0.2
     laser_model_type: "likelihood_field"
     max_beams: 60
     max_particles: 8000
@@ -32,12 +32,13 @@ amcl:
     tf_broadcast: true
     transform_tolerance: 1.0
     update_min_a: 0.2
-    update_min_d: 0.25
+    update_min_d: 0.2
     z_hit: 0.5
     z_max: 0.05
     z_rand: 0.5
     z_short: 0.05
-    set_initial_pose: true
+    scan_topic: scan
+    set_initial_pose: false
     initial_pose:
       x: 0.0
       y: 0.0

path_planner_server/config/controller.yaml

@@ -30,7 +30,7 @@ controller_server:
       min_vel_y: 0.0
       max_vel_x: 0.20
       max_vel_y: 0.0
-      max_vel_theta: 0.5
+      max_vel_theta: 0.3
       min_speed_xy: 0.0
       max_speed_xy: 0.25
       min_speed_theta: 0.0

robot_firmware/launch/table_detection.launch.py

@@ -0,0 +1,38 @@
+import os
+from launch import LaunchDescription
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    package_name = "robot_firmware"
+
+
+    # RVIZ configuration file
+    rviz_file = "rvis.rviz"
+    rviz_config_dir = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file)
+    rviz_config_dir = "/home/user/ros2_ws/src/rvis.rviz"
+
+    return LaunchDescription([
+
+        Node(
+            package=package_name, 
+            executable='table_detection', 
+            name='table_detection',
+            output='screen',
+            parameters=[{'use_sim_time': True}],
+        ),
+
+        Node(
+            package="rviz2",
+            executable="rviz2",
+            output="screen",
+            parameters=[{
+                "use_sim_time": True,
+            }],
+            arguments=[
+                "-d ", rviz_config_dir
+            ]
+        )
+
+    ]) 

robot_firmware/robot_firmware/action.py

@@ -0,0 +1,66 @@
+#! /usr/bin/env python3
+
+import rclpy
+from rclpy.action import ActionServer
+from rclpy.node import Node
+
+from robot_firmware_interfaces.action import AproachTable  # import the action message
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+
+
+class SimpleActionServer(Node):
+
+    def __init__(self):
+        super().__init__('main_node')
+        self._action_server = ActionServer(
+            self,
+            AproachTable,
+            'aproach_table',
+            self.execute_callback,
+            callback_group=MutuallyExclusiveCallbackGroup()
+        )
+
+    def execute_callback(self, goal_handle):
+        self.get_logger().info('Executing goal...')
+
+        # Perform the desired action here
+        # ...
+
+        # Check if the action has been canceled
+        if goal_handle.is_cancel_requested:
+            goal_handle.canceled()
+            self.get_logger().info('Goal canceled')
+            return
+
+        # Check if the action has been preempted
+        # if goal_handle.is_preempt_requested:
+        #     goal_handle.abort()
+        #     self.get_logger().info('Goal preempted')
+        #     return
+
+        # Set the result of the action
+        result = AproachTable.Result()
+        result.result = 1
+
+        goal_handle.succeed()
+        return result
+
+        self.get_logger().info('Goal completed')
+
+def main(args=None):
+    rclpy.init(args=args)
+    simple_action_server = SimpleActionServer()
+
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(simple_action_server)
+
+    executor.spin()
+    executor.shutdown()
+
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+
+

robot_firmware/robot_firmware/main_node.py

@@ -3,51 +3,214 @@
 import rclpy
 from rclpy.action import ActionServer
 from rclpy.node import Node
+from rclpy.duration import Duration
 
-from robot_firmware_interfaces.action import Empty  # import the action message
+from rclpy.action import ActionClient
 
+from robot_firmware_interfaces.action import AproachTable  # import the action message
+from geometry_msgs.msg import PoseStamped
+from action_msgs.msg import GoalStatus
 
-class SimpleActionServer(Node):
+from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
+from rclpy.executors import MultiThreadedExecutor
+
+from tf_transformations  import quaternion_from_euler
+
+# ros2 run robot_firmware main_node --ros-args -p use_sim_time:=true
+
+# Home position
+home_position = {
+    # [x, y, theta - angle to rotate when robot is under table]
+    "home_1": [7.25, -2.5, -1.57, 0.0],
+}
+
+# Shelf positions for picking
+trash_table_position = {
+    # [x, y, theta - angle to rotate when robot is under table]
+    "table_1": [-0.25, 1.20, 0.0, -1.57],
+}
+
+
+class MainNode(Node):
 
     def __init__(self):
         super().__init__('main_node')
-        self._action_server = ActionServer(
-            self,
-            Empty,
-            'remove_trash_table',
-            self.execute_callback)
-
-    def execute_callback(self, goal_handle):
-        self.get_logger().info('Executing goal...')
-
-        # Perform the desired action here
-        # ...
-
-        # Check if the action has been canceled
-        if goal_handle.is_cancel_requested:
-            goal_handle.canceled()
-            self.get_logger().info('Goal canceled')
+
+        self.navigator = BasicNavigator()
+        self.state = ""
+
+        self.get_logger().info('Waiting for navigattion to start...')
+        self.navigator.waitUntilNav2Active()
+
+        # Create action client
+        self.action_cli = ActionClient(self, AproachTable, 'aproach_table')
+
+        # Create dummy timer
+        self.timer = self.create_timer(1.0, self.timer_clb, callback_group=MutuallyExclusiveCallbackGroup())
+        self.action_status = False
+
+        # self.get_logger().info('Initializing robot pose...')
+        # self.set_initial_position()
+
+        self.get_logger().info('Main node started')
+
+
+    def timer_clb(self):
+        self.timer.cancel()
+        self.action_status = False
+
+        # Step 1 - go to known location where table might be
+        table_loc = trash_table_position['table_1']
+        result = self.go_to_pose(table_loc[0], table_loc[1], table_loc[2])
+        if result != TaskResult.SUCCEEDED:
+            print(f"Step 1 - go to known location where table might be: FAILED {result}")
+            # TODO process failed task
+            return
+        print("Step 1 - go to known location where table might be: DONE")
+
+
+        # Step 2 - trye to approach table
+        (status, result) = self.aproach_table(dummy_aproach=True)
+        if status != GoalStatus.STATUS_SUCCEEDED :
+            print(f"action client failed: {status}")
+            # TODO process failed task
+            return
+
+        # 0 - success, 1 - table not found, 2 - failed approach
+        if result.result:
+            print(f"Action cliented returned: {result.result}")
+            # TODO process failed task -  go home
+            return
+
+        print(f"action server finished with result {result.result}")
+
+
+         # Step 3 - Rotate robot
+        result = self.rotate_robot(yaw=table_loc[3])
+        if not result:
+            print(f"Step 3 - Rotate robot: FAILED")
+            # TODO process failed task
+            return
+        self.create_rate(1/(5*abs(table_loc[3]))).sleep()
+        print(f"Step 3 - Rotate robot: SUCCESS")
+
+        # Step 4 - Lift elevator
+        # Step 4.1 - Change footprint
+
+
+        # Step 5 - Move table to back room
+
+
+        # Step 6 - Down elevator
+        # Step 6.1 - Change footprint
+
+
+        # Step 7 - Get out of table
+
+
+        # Step 7 - Go to home position  
+        home_loc = home_position['home_1']
+        result = self.go_to_pose(home_loc[0], home_loc[1], home_loc[2])
+        if result != TaskResult.SUCCEEDED:
+            print(f"Step 7 - go to home position: FAILED {result}")
+            # TODO process failed task
             return
+        print("Step 7 - go to home positio: DONE")
+
 
-        # Check if the action has been preempted
-        # if goal_handle.is_preempt_requested:
-        #     goal_handle.abort()
-        #     self.get_logger().info('Goal preempted')
-        #     return
 
-        # Set the result of the action
-        result = Empty.Result()
-        result.result = True
 
-        goal_handle.succeed()
+
+
+
+
+    def set_initial_position(self):
+        print("Set initial position")
+        # Set your demo's initial pose
+        initial_pose = PoseStamped()
+        initial_pose.header.frame_id = 'map'
+        initial_pose.header.stamp = self.get_clock().now().to_msg()
+        initial_pose.pose.position.x = 0.0
+        initial_pose.pose.position.y = 0.0
+        initial_pose.pose.orientation.x = 0.0
+        initial_pose.pose.orientation.y = 0.0
+        initial_pose.pose.orientation.z = 0.0
+        initial_pose.pose.orientation.w = 1.0
+
+        self.navigator.setInitialPose(initial_pose)
+
+    def go_to_pose(self, x, y, yaw):
+        shelf_item_pose = PoseStamped()
+        shelf_item_pose.header.frame_id = 'map'
+        shelf_item_pose.header.stamp = self.get_clock().now().to_msg()
+        shelf_item_pose.pose.position.x = x
+        shelf_item_pose.pose.position.y = y
+
+        q = quaternion_from_euler(0, 0, yaw)
+
+        shelf_item_pose.pose.orientation.x = q[0]
+        shelf_item_pose.pose.orientation.y = q[1]
+        shelf_item_pose.pose.orientation.z = q[2]
+        shelf_item_pose.pose.orientation.w = q[3]
+        print(f'Received request for item picking at {shelf_item_pose.pose}.')
+        self.navigator.goToPose(shelf_item_pose)
+
+        # Do something during our route
+        # (e.x. queue up future tasks or detect person for fine-tuned positioning)
+        # Simply print information for workers on the robot's ETA for the demonstation
+        i = 0
+        while not self.navigator.isTaskComplete():
+            i += 1
+            feedback = self.navigator.getFeedback()
+            if feedback and i % 20 == 0:
+                # print(
+                #     'Estimated time of arrival at '
+                #     + str(shelf_item_pose.pose)
+                #     + ' for worker: '
+                #     + '{0:.0f}'.format(
+                #         Duration.from_msg(feedback.estimated_time_remaining).nanoseconds
+                #         / 1e9
+                #     )
+                #     + ' seconds.'
+                # )
+                print("Moving")
+
+        result = self.navigator.getResult()
         return result
+
+    def aproach_table(self, dummy_aproach):
+        goal_msg = AproachTable.Goal()
+        goal_msg.dummy_aproach = dummy_aproach
+
+        available = self.action_cli.wait_for_server(timeout_sec=3)
+        if not available:
+            return(-1, None)
+
+        res = self.action_cli.send_goal(goal_msg)
+        status = res.status
+        result = res.result
+        return (status, result)
+
+
+
+    def rotate_robot(self, yaw) -> bool:
+        # return True if SUCCESS else False
+        status = self.navigator.spin(spin_dist=yaw)
+        return status
 
-        self.get_logger().info('Goal completed')
 
 def main(args=None):
     rclpy.init(args=args)
-    simple_action_server = SimpleActionServer()
-    rclpy.spin(simple_action_server)
+    main_node = MainNode()
+
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(main_node)
+
+    executor.spin()
+    executor.shutdown()
+
+    rclpy.spin(main_node)
     rclpy.shutdown()
 
 if __name__ == '__main__':