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
No related branches found
No related tags found
No related merge requests found
...@@ -3,6 +3,7 @@ from rclpy.node import Node ...@@ -3,6 +3,7 @@ from rclpy.node import Node
from geometry_msgs.msg import PoseStamped from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker from visualization_msgs.msg import Marker
from tf_transformations import quaternion_from_euler from tf_transformations import quaternion_from_euler
from std_msgs.msg import Int32
import time import time
...@@ -15,7 +16,7 @@ class GoalSelector(Node): ...@@ -15,7 +16,7 @@ class GoalSelector(Node):
# Marker publisher for RViz2 visualization # Marker publisher for RViz2 visualization
self.marker_publisher = self.create_publisher(Marker, '/goal_markers', 10) 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 # Predefined goal points with positions and angles
self.goals = [ self.goals = [
{"x": 0.622, "y": -0.714, "angle": -1.600}, #1 {"x": 0.622, "y": -0.714, "angle": -1.600}, #1
...@@ -38,7 +39,7 @@ class GoalSelector(Node): ...@@ -38,7 +39,7 @@ class GoalSelector(Node):
self.get_logger().info("Goal Selector Node Started.") self.get_logger().info("Goal Selector Node Started.")
self.publish_goal_markers() # Visualize all goals in RViz2 self.publish_goal_markers() # Visualize all goals in RViz2
self.show_menu() # self.show_menu()
def publish_goal_markers(self): def publish_goal_markers(self):
...@@ -120,6 +121,8 @@ class GoalSelector(Node): ...@@ -120,6 +121,8 @@ class GoalSelector(Node):
print("Invalid choice, please try again.") print("Invalid choice, please try again.")
else: else:
print("Invalid input, please enter a number.") 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): def publish_goal(self, goal):
"""Publish the selected goal as a PoseStamped message.""" """Publish the selected goal as a PoseStamped message."""
......
...@@ -35,6 +35,13 @@ def generate_launch_description(): ...@@ -35,6 +35,13 @@ def generate_launch_description():
output='screen', output='screen',
emulate_tty=True emulate_tty=True
), ),
Node(
package='robobin',
executable='uwb_navigation_node',
name='uwb_navigation_node',
output='screen',
emulate_tty=True
)
# Add additional nodes # Add additional nodes
......
...@@ -14,12 +14,12 @@ def generate_launch_description(): ...@@ -14,12 +14,12 @@ def generate_launch_description():
), ),
Node( # Node(
package='robobin', # package='robobin',
executable='control_feedback', # executable='control_feedback',
name='control_feedback', # name='control_feedback',
output='screen', # output='screen',
emulate_tty=True # emulate_tty=True
), # ),
]) ])
\ No newline at end of file
...@@ -30,6 +30,7 @@ setup( ...@@ -30,6 +30,7 @@ setup(
'motor_node = robobin.motor_control_node:main', 'motor_node = robobin.motor_control_node:main',
'encoder_node = robobin.encoder:main', 'encoder_node = robobin.encoder:main',
'control_feedback = robobin.control_feedback: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