Moving a husky using the move_base action on ROS2 foxy

asked 2022-12-22 05:50:44 -0500

Roeyevgi gravatar image

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()

edit retag flag offensive close merge delete