Hi,
I tried to follow all the terminal commands on the workbook but the robot did not seem to respond. I have attached my terminal responses here.
Hi @jeffrey.jahja7, welcome to the community!
Based on your screenshot, I think you meant to post this under ROS2 Navigation Humble - The Construct ROS Community. Please keep in mind while asking for help in the forum, it helps us compartmentalize.
Usually the lifecycle_manager_navigation node log Managed nodes are active means everything launched correctly, so it is strange that the robot is not responding. Could you provide a little more context, as well as rviz with the appropriate configuration? Another screenshot would be great.
Does sending a goal through rviz work after launching Nav2?
Hi @rodrigo55, thank you for the greetings!
I did not configure anything on rviz. Here are the set of code that I should be entering as part of the worksheet.
cd ~/ros2_ws
colcon build
source install/setup.bash
ros2 launch leo_description loc_nav.launch.py
Execute in Terminal #2
ros2 run my_action_server action_server
Execute in Terminal #3
ros2 launch my_action_client example75_launch_file.launch.py
To update here is my code for my action client:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav_msgs.msg import Odometry
from gazebo_msgs.msg import ModelStates
import math
#from leo_description.action import GoToPose
from custom_interfaces.action import GoToPose
class CustomActionServer(Node):
def __init__(self):
super().__init__('custom_action_client')
self._action_client = ActionClient(self, GoToPose, 'go_to_pose')
# Subscribe to odometry to get the robot's position
self._odom_subscriber = self.create_subscription(
Odometry,
'/odom',
self.odom_callback,
10)
# Initialize positions
self.robot_x = None
self.robot_y = None
self.meteor_x = None
self.meteor_y = None
# Subscriber to track the meteor position
self.model_state_subscriber = self.create_subscription(
ModelStates,
'/gazebo/model_states',
self.model_states_callback,
10)
# Distance threshold to send new goal
self.near_meteor_threshold = 7.5 # Units (e.g., meters)
# Store goal handle to keep track of active goal
self.goal_handle = None
# Flag to indicate if new goal has been sent
self.new_goal_sent = False
def send_goal(self, x, y, yaw):
goal_msg = GoToPose.Goal()
goal_msg.x = x
goal_msg.y = y
goal_msg.yaw = yaw
self.get_logger().info(f'Sending goal: x={x}, y={y}, yaw={yaw}')
# Wait until the action server is available
self._action_client.wait_for_server()
# Send the goal asynchronously
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
# Attach callback for when the goal response is received
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
# Handle the goal response
self.goal_handle = future.result()
if not self.goal_handle.accepted:
self.get_logger().info('Goal was rejected by the action server.')
return
self.get_logger().info('Goal accepted by the action server.')
# Attach callback for when the result is received
self._get_result_future = self.goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
# Handle the result
result = future.result().result
self.get_logger().info(f'Action completed with success: {result.success}')
# Optionally, shutdown the node or send another goal
def feedback_callback(self, feedback_msg):
# Handle feedback (if any)
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback from action server.')
def odom_callback(self, msg):
# Update robot's position
self.robot_x = msg.pose.pose.position.x
self.robot_y = msg.pose.pose.position.y
# Check if we have both positions and haven't sent the new goal yet
if (self.robot_x is not None and self.robot_y is not None and
self.meteor_x is not None and self.meteor_y is not None and
not self.new_goal_sent):
# Calculate distance to meteor
distance = math.hypot(self.robot_x - self.meteor_x, self.robot_y - self.meteor_y)
if distance <= self.near_meteor_threshold:
self.get_logger().info('Robot is near meteor. Sending new goal to avoid it.')
# Send new goal
self.send_goal(-11.5, -3.5, -1.5) # next coordinates of lost astronaut
self.new_goal_sent = True # Prevent sending multiple goals
def model_states_callback(self, msg):
# Extract the meteor's position from the ModelStates message
# Get index of the meteor in the model names list
meteor_index = msg.name.index('meteor')
# Get the meteor's position
self.meteor_x = msg.pose[meteor_index].position.x
self.meteor_y = msg.pose[meteor_index].position.y
def main(args=None):
rclpy.init(args=args)
action_client = CustomActionServer()
# Send the initial goal
initial_x = -8.0
initial_y = 6.0
initial_yaw = 1.57
action_client.send_goal(initial_x, initial_y, initial_yaw)
# Keep the node alive to receive callbacks
rclpy.spin(action_client)
action_client.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Please provide more context.
- Which course are you taking? It looks like that’s not clear here.
- Which unit are you on, and which specific exercise or example are you following?
- How did you expect the robot to respond?
- Any other context that could help us to understand where you are and what you are doing.
Hi @bayodesegun
-
Which course are you taking? It looks like that’s not clear here.
I am taking the ROS2 Basics in 5 Days -
Which unit are you on, and which specific exercise or example are you following?
-
Currently at the Actions Section 7, Namely 7.8 Writing a custom action client
-
How did you expect the robot to respond?
The robot should respond by navigating to the target area as seen in this gif.
-
Any other context that could help us to understand where you are and what you are doing.
I had copied and pasted the required code for example 7.5 and the required terminal commands for the example.
Thank you
Okay, thanks for clarifying. Please check the following, in the order presented:
- Check that you have copied all sample code blocks correctly.
- Recompile the whole workspace. Sometimes, when making incremental changes, the latest changes are not compiled because the package was previously compiled:
cd ros2_ws
rm -rf build/ install/ log/
colcon build
source install/setup.bash
- Before running other commands on the 2nd, 3rd, … terminals, source the changes on the first terminal
source ~/ros2_ws/install/setup.bash
Kindly let us know if this helps.
Hi @bayodesegun,
This command
ros2 launch leo_description loc_nav.launch.py
Did not work. Do you think I should use a different source code? Namely:
source /home/simulations/ros2_sims_ws/install/setup.bash
This should help with the leo_description part of the module not found error.
The custom_interfaces error indicates that the package could not be found in your workspace, and it’s a dependency of the package you are trying to launch.
Hi @bayodesgun,
I have recently fixed the custom_interfaces problem but having said so, I am having this problem with Terminal #1. it seems like whatever command that I have sent from Terminal #3, ros2 launch my_action_client example75_launch_file.launch.py does nothing. Could you help me understand why is this the case? When I pressed Ctrl+C, it seems like it has a problem with this line:
self._action_client.wait_for_server()
Terminal #1:
Terminal #3:
I did a bit of my own debugging and it seems like the action server is not available.
Sorry for the delayed response.
Things you can check, in order:
- Before starting the client on Terminal 3, ensure you run
source ~/ros2_ws/install/setup.bash - Run
ros2 topic listand confirm the action topics are present.- If not, please check that your action server is properly written
- Check that your action client is properly written, following the examples given in the notes
catkin_ws does not exist and poses an error
the command rostopic is also not found
Huh, I gave ROS1 commands instead of ROS2 
. I have corrected it.
Hi @bayodesgun,
I still seem to have issues with my code. I have copied the code from the notes for both the action server and client.
Client:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav_msgs.msg import Odometry
from gazebo_msgs.msg import ModelStates
import math
#from leo_description.action import GoToPose
from custom_interfaces.action import GoToPose
class CustomActionServer(Node):
def __init__(self):
super().__init__('custom_action_client')
self._action_client = ActionClient(self, GoToPose, 'go_to_pose')
# Subscribe to odometry to get the robot's position
self._odom_subscriber = self.create_subscription(
Odometry,
'/odom',
self.odom_callback,
10)
# Initialize positions
self.robot_x = None
self.robot_y = None
self.meteor_x = None
self.meteor_y = None
# Subscriber to track the meteor position
self.model_state_subscriber = self.create_subscription(
ModelStates,
'/gazebo/model_states',
self.model_states_callback,
10)
# Distance threshold to send new goal
self.near_meteor_threshold = 7.5 # Units (e.g., meters)
# Store goal handle to keep track of active goal
self.goal_handle = None
# Flag to indicate if new goal has been sent
self.new_goal_sent = False
def send_goal(self, x, y, yaw):
goal_msg = GoToPose.Goal()
goal_msg.x = x
goal_msg.y = y
goal_msg.yaw = yaw
self.get_logger().info(f'Sending goal: x={x}, y={y}, yaw={yaw}')
# Wait until the action server is available
timeout = 10 # 10 seconds timeout for server connection
if not self._action_client.wait_for_server(timeout_sec=timeout):
self.get_logger().warn(f"Action server not available after {timeout} seconds!")
return
# Send the goal asynchronously
self._send_goal_future = self._action_client.send_goal_async(
goal_msg, feedback_callback=self.feedback_callback)
# Attach callback for when the goal response is received
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
# Handle the goal response
self.goal_handle = future.result()
if not self.goal_handle.accepted:
self.get_logger().info('Goal was rejected by the action server.')
return
self.get_logger().info('Goal accepted by the action server.')
# Attach callback for when the result is received
self._get_result_future = self.goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
# Handle the result
result = future.result().result
self.get_logger().info(f'Action completed with success: {result.success}')
# Optionally, shutdown the node or send another goal
def feedback_callback(self, feedback_msg):
# Handle feedback (if any)
feedback = feedback_msg.feedback
self.get_logger().info('Received feedback from action server.')
def odom_callback(self, msg):
# Update robot's position
self.robot_x = msg.pose.pose.position.x
self.robot_y = msg.pose.pose.position.y
# Check if we have both positions and haven't sent the new goal yet
if (self.robot_x is not None and self.robot_y is not None and
self.meteor_x is not None and self.meteor_y is not None and
not self.new_goal_sent):
# Calculate distance to meteor
distance = math.hypot(self.robot_x - self.meteor_x, self.robot_y - self.meteor_y)
if distance <= self.near_meteor_threshold:
self.get_logger().info('Robot is near meteor. Sending new goal to avoid it.')
# Send new goal
self.send_goal(-11.5, -3.5, -1.5) # next coordinates of lost astronaut
self.new_goal_sent = True # Prevent sending multiple goals
def model_states_callback(self, msg):
# Extract the meteor's position from the ModelStates message
# Get index of the meteor in the model names list
meteor_index = msg.name.index('meteor')
# Get the meteor's position
self.meteor_x = msg.pose[meteor_index].position.x
self.meteor_y = msg.pose[meteor_index].position.y
def main(args=None):
rclpy.init(args=args)
action_client = CustomActionServer()
# Send the initial goal
initial_x = -8.0
initial_y = 6.0
initial_yaw = 1.57
action_client.send_goal(initial_x, initial_y, initial_yaw)
# Keep the node alive to receive callbacks
rclpy.spin(action_client)
action_client.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
And server:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient, ActionServer
from tf_transformations import quaternion_from_euler
from action_msgs.msg import GoalStatus
import time
from leo_description.action import GoToPose
class MyActionServer(Node):
def __init__(self):
super().__init__('my_action_server')
# Action server to accept goals
self.action_server = ActionServer(
self, GoToPose, 'go_to_pose', self.execute_callback)
# Publisher to set initial pose
self.initial_pose_publisher = self.create_publisher(
PoseWithCovarianceStamped, '/initialpose', 10)
# Action client for navigation to pose
self.nav_to_pose_client = ActionClient(
self, NavigateToPose, '/navigate_to_pose')
# Wait for the localization node to be ready
self.wait_for_localization()
# Set the initial pose to (0, 0, 0)
self.set_initial_pose(0.0, 0.0, 0.0)
def wait_for_localization(self):
self.get_logger().info("Waiting for localization to be active...")
amcl_pose_topic = '/amcl_pose'
while not self.count_subscribers('/initialpose') > 0:
self.get_logger().info("Waiting for subscribers to /initialpose...")
time.sleep(1.0)
while not self.topic_exists(amcl_pose_topic):
self.get_logger().info(f"Waiting for {amcl_pose_topic} topic...")
time.sleep(1.0)
self.get_logger().info("Localization is active.")
def topic_exists(self, topic_name):
topics = self.get_topic_names_and_types()
return any(topic[0] == topic_name for topic in topics)
def set_initial_pose(self, x, y, yaw):
"""Publish the initial pose to the /initialpose topic multiple times."""
initial_pose = PoseWithCovarianceStamped()
initial_pose.header.frame_id = 'map'
initial_pose.header.stamp = self.get_clock().now().to_msg()
initial_pose.pose.pose.position.x = x
initial_pose.pose.pose.position.y = y
# Set the orientation as quaternion (for yaw rotation)
q = quaternion_from_euler(0, 0, yaw)
initial_pose.pose.pose.orientation.x = q[0]
initial_pose.pose.pose.orientation.y = q[1]
initial_pose.pose.pose.orientation.z = q[2]
initial_pose.pose.pose.orientation.w = q[3]
# Publish the initial pose multiple times
for i in range(10):
self.initial_pose_publisher.publish(initial_pose)
self.get_logger().info(f"Publishing initial pose ({i+1}/10)")
time.sleep(0.1)
self.get_logger().info(f"Initial pose set to x: {x}, y: {y}, yaw: {yaw}")
async def execute_callback(self, goal_handle):
"""Handle the incoming goal request."""
self.get_logger().info('Received goal request')
x = goal_handle.request.x
y = goal_handle.request.y
yaw = goal_handle.request.yaw
self.get_logger().info(f"Navigating to: x={x}, y={y}, yaw={yaw}")
# Send navigation goal
result = await self.send_navigation_goal(x, y, yaw)
if result:
self.get_logger().info('Goal reached successfully!')
goal_handle.succeed()
return GoToPose.Result(success=True)
else:
self.get_logger().info('Failed to reach goal :(')
goal_handle.abort()
return GoToPose.Result(success=False)
async def send_navigation_goal(self, x, y, yaw):
"""Send a navigation goal using the NavigateToPose action."""
goal_msg = NavigateToPose.Goal()
# Set goal position
goal_msg.pose.pose.position.x = x
goal_msg.pose.pose.position.y = y
# Set goal orientation (yaw as quaternion)
q = quaternion_from_euler(0, 0, yaw)
goal_msg.pose.pose.orientation.x = q[0]
goal_msg.pose.pose.orientation.y = q[1]
goal_msg.pose.pose.orientation.z = q[2]
goal_msg.pose.pose.orientation.w = q[3]
# Set the frame and timestamp
goal_msg.pose.header.frame_id = 'map'
goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
# Wait for the NavigateToPose action server
self.get_logger().info("Waiting for the NavigateToPose action server...")
self.nav_to_pose_client.wait_for_server()
# Send the goal
self.get_logger().info(f"Sending navigation goal to: x={x}, y={y}, yaw={yaw}")
send_goal_future = self.nav_to_pose_client.send_goal_async(goal_msg)
nav_goal_handle = await send_goal_future
if not nav_goal_handle.accepted:
self.get_logger().info('Navigation goal rejected')
return False
self.get_logger().info('Navigation goal accepted')
# Wait for result
get_result_future = nav_goal_handle.get_result_async()
nav_result = await get_result_future
if nav_result.status == GoalStatus.STATUS_SUCCEEDED:
self.get_logger().info('Navigation succeeded')
return True
else:
self.get_logger().info(f'Navigation failed with status: {nav_result.status}')
return False
def main(args=None):
rclpy.init(args=args)
node = MyActionServer()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Is it because I have sourced the bash codes from different origins?
I can’t seem to find the go_to_pose node either
You did not state the specific issues you are still having, so I don’t know how to help.
If you’re expected to import GoToPose from leo_description.action, then you need to source the two workspaces on the terminal where you want to run the code:
source /home/simulations/ros2_sims_ws/install/setup.bash
source `/ros2_ws/install/setup.bash
Hi @bayodesegun,
unfortnately the GoToPose node is not available despite having sourced the two workspaces stated
The main issue is that the code from Terminal #3 (Client code) was unable to find the server. Here is the output after modifying the example code slightly using ChatGPT.
It seems like the ActionClient node is not communicating properly
I dont know what specifically the issue is but it seems to be with regards to this line self._action_client.wait_for_server()