I’m doing last part of the Rosject where I’m asked to create a service server and an action client to move the robot to different poses in the map.
I created my service server which when called will read from a file the pose data and then through a topic send it to my action client. In my action client, I subscribe to that topic, and in the subscriber callback I’m sending my goal to the move_base/goal topic using an action client. But for some reason my robot never reaches the goal. It moves a bit, then start to rotate in-place.
Here’s the code for my action client:
#! /usr/bin/env python
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal#, MoveBaseResult, MoveBaseFeedback
import time
from geometry_msgs.msg import PoseStamped
# definition of the feedback callback. This will be called when feedback
# is received from the action server
# it just prints a message indicating a new message has been received
def feedback_callback(feedback):
print("Going to new position")
#Subscriber callback
def send_new_goal(msg):
# creates a goal to send to the action server
print("New goal received")
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = msg.header.frame_id
goal.target_pose.pose.position.x = msg.pose.position.x
goal.target_pose.pose.position.y = msg.pose.position.y
goal.target_pose.pose.position.z = msg.pose.position.z
goal.target_pose.pose.orientation.x = msg.pose.orientation.x
goal.target_pose.pose.orientation.y = msg.pose.orientation.y
goal.target_pose.pose.orientation.z = msg.pose.orientation.z
goal.target_pose.pose.orientation.w = msg.pose.orientation.w
global client
print("Sending goal to action_server")
client.send_goal(goal, feedback_cb=feedback_callback)
state_result = client.get_state()
rate = rospy.Rate(1)
rospy.loginfo("state_result: "+str(state_result))
DONE = 2
while state_result < DONE:
rospy.loginfo("Doing Stuff while waiting for the Server to give a result....")
rate.sleep()
state_result = client.get_state()
rospy.loginfo("state_result: "+str(state_result))
# initializes the action client node
rospy.init_node('goal_action_server')
action_server_name = '/move_base'
client = actionlib.SimpleActionClient(action_server_name, MoveBaseAction)
sub = rospy.Subscriber('/spot_to_go', PoseStamped, callback=send_new_goal)
# waits until the action server is up and running
rospy.loginfo('Waiting for action Server ' + action_server_name)
client.wait_for_server()
rospy.loginfo('Action Server Found...' + action_server_name)
rospy.spin()
Can someone help me identifying what I’m doing wrong?
I also have some additional questions regarding my code:
Is there a way to copy the pose data directly from the subscriber callback msg variable without having to access individually the pose.position.x, pose.orientation.z, etc?
Does using classes gives me any advantage? Besides making my code more structured and modular
Is it recommended to integrate everything in only 1 file? Or is it a best practice to fragment code as much as possible?
I am currently in the final chapter of this course also. I have experienced similar problem when I worked on the previous units.
The main cause for the robot to keep rotating in place, I believe, is lack of good map and poor localization.
Try to re-map the environment / arena again - but this time do it slowly so that every part of the map is densely mapped.
Use more particles in the amcl - implement global_localization before the robot starts to do any goal planning moves.
Modify location and map update variables to update the map faster in amcl node options (remember the params yaml file?).
EDIT Also, change move_base parameters so update is faster.
That would be all my suggestions for you.
Regarding your questions:
I do not think there is a way to do that. You can copy the pose.position and pose.orientation to geometry_msgs/Pose and geometry_msgs/Quaternion respectively, but to access those variables (read or write) has to be done individually. [In my opinion.]
Honestly, no. If your code is not working in a no-class version, a classed version will have no improvements. Other than being more structured and modular as you said.
I would advise you to keep each part of the rosject in separate files that can be initialized using one launch file. Combine only that parts that are necessary (like service and action clients) into your main code. [In my opinion.]
The funny thing is that when I use Rviz to manually select the initial and goal pose, my robot reaches the goal as intended.
But when I send the goal using the action client, my robot begins to move for like 5 sec, then it start behaving odd and eventually rotates in-place, without ever reaching the goal.
My initial thought was that maybe it was due to the ROS navigation stack and my action client nodes colliding with one another because they are running concurrently. But maybe there’s another explanation?
Because if I echo my topic “spot_to_go” it is properly built. That’s why I think my error is inside my action_client node.
Are you sure that only one client is sending goal poses? Make sure that only one client is connected to the goal action server.
Since your send_new_goal() is a callback, I believe that the program is being called repeatedly (as it should) with the same goal points, eventually making the data corrupted after that 5 seconds of moving.
Try to implement a logic where if the current goal message is same as previous then do not execute the goal a second time. If the goal message is not the same as the previous goal message then execute this goal just once and wait for it to complete.
I guess that is what is happening, the data is sent too fast that the robot cannot decide when to stop and when to move and thus starts spinning.
Try a different approach perhaps - instead of reading goals from a text file and creating a publisher-subscriber model, use a service server-client model.
I did some testing and found out what my problem was. It had to do with the move_base parameters.
Some of the spots I recorded in previous sections collided with the inflation radius of my map. And since I was using the navfn global planner. The algorithm was never able to send my robot to that place, so it initiated the recovery behaviors (rotating). When I changed my global planner to carrot planner it was able to approach the spot I indicated.
So, my problem had nothing to do with the action client and service server, apparently.
However, I decided to do more testing and found out that my robot still didn’t performed well in the map (it takes a lot of time to reach the goal, the global planner plan is telling my robot to go through obstacles, etc). So I have to tune my navigation parameters. Any advice on that?
There are so many parameters that I don’t even know where to start and the course only mentions a few of them.
I think you might have to tune your Goal Tolerance Parameters in the move_base parameters file.
This would be my immediate assumption though. You need to choose between TrajectoryPlanner and DWAPlanner (I guess the other two - eband and teb would not be much helpful).
Also, play around with the Forward Simulation and Trajectory Scoring parameters.
At the moment, I have only completed about half of the final chapter of ROS Navigation. I cannot help you much with Inflation concept as of now.
Go to the ROS documentation page for those topics to see the parameters. You can then add them to the yaml file and play around with the values.