From 78fe401c79465a81c9db325693c892e33e29804e Mon Sep 17 00:00:00 2001
From: Paul-Winpenny <92634321+Paul-Winpenny@users.noreply.github.com>
Date: Thu, 16 Jan 2025 09:44:53 +0000
Subject: [PATCH] Integrated goal_selector into the wider system.
---
.../src/super_robot/super_robot/goal_selector.py | 7 +++++--
ros2/src/robobin/launch/robobin_launch.py | 7 +++++++
.../robobin/launch/robobin_no_components_launch.py | 14 +++++++-------
ros2/src/robobin/setup.py | 1 +
4 files changed, 20 insertions(+), 9 deletions(-)
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 9bbdc8df..c7834685 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 50e80250..8fae6e6b 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 725aab91..f6cbd691 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 489980a1..67167e89 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",
],
},
)
--
GitLab