Robotics StackExchange | Archived questions

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

Answers