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 movebase action (ros1 noetic), but the robot doesn't seem to respond to the action. It only responds when I publish to the /movebasesimple/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.actionclient.waitforserver() it does not seem to continue since the 'b' print is not showing up on the screen. I tried to use the rosbrige 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 movebasemsgs.action import MoveBase from geometry_msgs.msg import PoseStamped import math
DISTANCE_TOLERANCE = 0.1
class MovementActionClient(Node): def init(self): super().init('movementactionclientnode') self.actionclient = ActionClient(self,MoveBase, '/movebase')
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()
Asked by Roeyevgi on 2022-12-22 06:50:44 UTC
Comments