Hi The Construct team,
I am working on Exercise 4.4 of Path Planning 1 chapter. The (husky) robot is still not moving to the goal position. I think the robot is not responding to the action goal command.
I first run this command on webshell 1:
roslaunch husky_navigation move_base_demo.launch
On webshell 2, I run this command:
rosrun send_goals send_goal_client.py
Here is my code for send_goal_client.py
:
#! /usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
class SendGoalsClient:
def __init__(self):
self.action_name = "/move_base"
self.action_client = actionlib.SimpleActionClient(ns=self.action_name,
ActionSpec=MoveBaseAction)
print("Action Client Initialized")
print("Waiting for Action Server...")
self.action_client.wait_for_server()
print("Action Server Started and Running")
# goal = (pos_x, pos_y, quat_z, quat_w)
self.goals = [(0.0, -0.05, -0.90, 0.44),
(-3.0, -4.0, 0.015, 0.99),
(2.0, -4.0, 0.85, 0.55)]
return None
def update_goal(self, pos_x, pos_y, quat_z, quat_w):
self.goal = MoveBaseGoal()
self.goal.target_pose.pose.position.x = pos_x
self.goal.target_pose.pose.position.y = pos_y
self.goal.target_pose.pose.orientation.z = quat_z
self.goal.target_pose.pose.orientation.w = quat_w
return None
def send_goal(self):
self.action_client.send_goal(
self.goal, feedback_cb=self.feedback_callback)
print("Goal Sent")
return None
def feedback_callback(self, feedback):
self.feedback = feedback
print("Feedback:")
print(feedback)
return None
def get_result(self):
result = self.action_client.get_result()
print("Result:")
print(result)
return None
def main(self):
# single loop for now - must be infinte loop.
for goal in self.goals:
pos_x, pos_y, quat_z, quat_w = goal
self.update_goal(pos_x, pos_y, quat_z, quat_w)
self.send_goal()
self.action_client.wait_for_result() # wait for robot to reach goal
self.action_client.get_result()
return None
if __name__ == "__main__":
rospy.init_node("send_goals_action_client")
action_client = SendGoalsClient()
action_client.main()
# End of Code
I need some assistance getting this to work. Also let me know if anything is wrong in my code.
Thanks,
Girish