diff --git a/Robobin_Lidar_Navigation/src/super_robot/super_robot/goal_selector.py b/Robobin_Lidar_Navigation/src/super_robot/super_robot/goal_selector.py index 9bbdc8df196796d778596a5e61d5b370bcea75aa..c783468510f8bdf3be46e23699b09ae65f21b5f9 100644 --- a/Robobin_Lidar_Navigation/src/super_robot/super_robot/goal_selector.py +++ b/Robobin_Lidar_Navigation/src/super_robot/super_robot/goal_selector.py @@ -3,6 +3,7 @@ from rclpy.node import Node from geometry_msgs.msg import PoseStamped from visualization_msgs.msg import Marker from tf_transformations import quaternion_from_euler +from std_msgs.msg import Int32 import time @@ -15,7 +16,7 @@ class GoalSelector(Node): # Marker publisher for RViz2 visualization self.marker_publisher = self.create_publisher(Marker, '/goal_markers', 10) - + self.call_subscriber = self.create_subscription(Int32, '/nav_command_lidar', self.handle_call, 10) # Predefined goal points with positions and angles self.goals = [ {"x": 0.622, "y": -0.714, "angle": -1.600}, #1 @@ -38,7 +39,7 @@ class GoalSelector(Node): self.get_logger().info("Goal Selector Node Started.") self.publish_goal_markers() # Visualize all goals in RViz2 - self.show_menu() + # self.show_menu() def publish_goal_markers(self): @@ -120,6 +121,8 @@ class GoalSelector(Node): print("Invalid choice, please try again.") else: print("Invalid input, please enter a number.") + def handle_call(self, msg): + self.publish_goal(self.goals[msg.data - 1]) def publish_goal(self, goal): """Publish the selected goal as a PoseStamped message.""" diff --git a/ros2/src/robobin/launch/robobin_launch.py b/ros2/src/robobin/launch/robobin_launch.py index 50e80250ab233bc37ef956ae088274e6912ecc68..8fae6e6b25e14ae5a875d35637a54126df60e679 100755 --- a/ros2/src/robobin/launch/robobin_launch.py +++ b/ros2/src/robobin/launch/robobin_launch.py @@ -35,6 +35,13 @@ def generate_launch_description(): output='screen', emulate_tty=True ), + Node( + package='robobin', + executable='uwb_navigation_node', + name='uwb_navigation_node', + output='screen', + emulate_tty=True + ) # Add additional nodes diff --git a/ros2/src/robobin/launch/robobin_no_components_launch.py b/ros2/src/robobin/launch/robobin_no_components_launch.py index 725aab91b4fd571cb3f61b992b5b6be1706aef6d..f6cbd691b21e1e74f229e83057cc9b2aac103ff5 100644 --- a/ros2/src/robobin/launch/robobin_no_components_launch.py +++ b/ros2/src/robobin/launch/robobin_no_components_launch.py @@ -14,12 +14,12 @@ def generate_launch_description(): ), - Node( - package='robobin', - executable='control_feedback', - name='control_feedback', - output='screen', - emulate_tty=True - ), + # Node( + # package='robobin', + # executable='control_feedback', + # name='control_feedback', + # output='screen', + # emulate_tty=True + # ), ]) \ No newline at end of file diff --git a/ros2/src/robobin/setup.py b/ros2/src/robobin/setup.py index 489980a1de2829a169ae52292f59fee539b65ffa..67167e89385117f85e338650698e1a05bcfa60cc 100644 --- a/ros2/src/robobin/setup.py +++ b/ros2/src/robobin/setup.py @@ -30,6 +30,7 @@ setup( 'motor_node = robobin.motor_control_node:main', 'encoder_node = robobin.encoder:main', 'control_feedback = robobin.control_feedback:main', + "uwb_navigation_node = robobin.uwb_navigation_node:main", ], }, )