Moving a husky using the move_base action on ROS2 foxy
Hello ROS community,
I am trying to write an action client node using python (ros2 foxy) to move a husky robot using the move_base action (ros1 noetic), but the robot doesn't seem to respond to the action. It only responds when I publish to the /move_base_simple/goal topic. Also publishing to this topic works using Rviz. As you can see in the code below there are 2 print statements of 'a' and 'b'. when the program gets to self._action_client.wait_for_server() it does not seem to continue since the 'b' print is not showing up on the screen. I tried to use the ros_brige but without success.
Does anyone know what the problem is?
Thank you in advance. Roey.
! /usr/bin/env python3
import rclpy from rclpy.node import Node
from action_msgs.msg import GoalStatus
from rclpy.action import ActionClient
from efr_rsm.action import Nav2point
from move_base_msgs.action import MoveBase from geometry_msgs.msg import PoseStamped import math
DISTANCE_TOLERANCE = 0.1
class MovementActionClient(Node): def __init__(self): super().__init__('movement_action_client_node') self._action_client = ActionClient(self,MoveBase, '/move_base')
def send_goal(self,x, y):
goal_msg = MoveBase.Goal()
goal_msg.target_pose.header.frame_id = 'odom'
goal_msg.target_pose.header.stamp = self.get_clock().now().to_msg()
goal_msg.target_pose.pose.position.x = float(x)
goal_msg.target_pose.pose.position.y = float(y)
goal_msg.target_pose.pose.position.z = 0.0
goal_msg.target_pose.pose.orientation.x = 0.0
goal_msg.target_pose.pose.orientation.y = 0.0
goal_msg.target_pose.pose.orientation.z = 0.0
goal_msg.target_pose.pose.orientation.w = 1.0
print('a')
self._action_client.wait_for_server()
print('b')
self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
self._send_goal_future.add_done_callback(self.goal_response_callback)
def feedback_callback(self):
feedback = MoveBase.Feedback()
print("Recieved Feedback: " + str(feedback.action_feedback))
def goal_response_callback(self, future):
goal_handle = future.result()
if goal_handle.accepted == False:
print('Goal Rejected')
return None
print('Goal Accepted')
self._goal_result_future = goal_handle.get_result_async()
self._goal_result_future.add_done_callback(self.get_result_callback)
def get_result_callback(self, future):
result = future.result().result
print('Result: ' + str(result.action_result))
rclpy.shutdown()
def main(): rclpy.init()
action_clinet = MovementActionClient()
print('Action Client is running...')
try:
x = input('Enter x coordinate: ')
y = input('Enter y coordinate: ')
action_clinet.send_goal(x,y)
rclpy.spin(action_clinet)
except KeyboardInterrupt:
action_clinet._action_client.destroy()
action_clinet.destroy_node()
rclpy.shutdown()
if __name__ == '__main__': main()