Skip to content
Snippets Groups Projects
Commit 78fe401c authored by Paul-Winpenny's avatar Paul-Winpenny
Browse files

Integrated goal_selector into the wider system.

parent ef68fdd1
Branches
No related tags found
No related merge requests found
......@@ -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."""
......
......@@ -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
......
......@@ -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
......@@ -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",
],
},
)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment