I have gotten as far as saving multiple spots to my spots_saved.txt file with the following sample data:
### What the F?!? ###
Room-1:
position:
x: -2.76009410711
y: -3.60989414856
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.000899836262279
w: 0.999999595147
Start:
position:
x: -0.884343746955
y: -0.794928654583
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.0295379308116
w: 0.999563660125
...
I tried using the position and orientation data from each of the labels to create a pose object and move my summit robot but it moved too quickly and to the wrong please.
Then I noticed the orientation section is a quaterniion and I tried converting it to an euler value but the robot movement was still quick, eratic and incorrect.
I’m using the following code:
#! /usr/bin/env python
import rospy
import actionlib
from my_summit_navigation.msg import PositionAction, PositionActionFeedback, PositionActionGoal, PositionActionResult, PositionFeedback, PositionGoal, PositionResult
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion
class SummitNavigationClient() :
def __init__(self) :
rospy.loginfo('... navigation-client starting')
self.move = Twist()
self.pub_move = rospy.Publisher('/summit_xl_control/cmd_vel', Twist, queue_size=10)
client = actionlib.SimpleActionClient('navigation_server', PositionAction)
client.wait_for_server()
goal = PositionGoal()
goal.label = 'Corner-4'
client.send_goal(goal)
client.wait_for_result()
result = client.get_result()
# rospy.loginfo(result)
test = euler_from_quaternion([0.0, 0.0, -0.99976283963, 0.0217776145328]) # manually copied x, y, z, w orientation values from one of my Corner-4 saved spot
rospy.loginfo(test)
self.move.linear.x = 5.81875368865 # same position value from my Corner-4 saved spot
self.move.linear.y = -5.95001714238 # same position value from my Corner-4 saved spot
self.move.linear.z = 0.0 # same position value from my Corner-4 saved spot
self.move.angular.x = 0.0 # pasted value returned from the euler_from_quaternion function above
self.move.angular.y = 0.0 # pasted value returned from the euler_from_quaternion function above
self.move.angular.z = -3.098033981006108 # pasted value returned from the euler_from_quaternion function above
while not rospy.is_shutdown() :
self.pub_move.publish(self.move)
if name == ‘main’ :
rospy.init_node('my_summit_navigation_client')
try :
navigator = SummitNavigationClient()
rospy.spin()
except rospy.ROSInterruptException as e :
rospy.logerr('Something went wrong: %s', e)
I need help understanding what I’m doing wrong so I can get my robot moves to my pre-saved spots, please.
These values don’t represent positions in the space, but linear and angular velocities: geometry_msgs/Twist Documentation
So basically, you are telling your robot to move at a linear velocity of almost 6 m/s.
I think you should start by doing the ROS Basics and ROS Navigation courses to get a better understanding of the basics.
Best,
I was getting it wrong by attempting to issue a Twist command. What I was meant to be issueing position and orientation goal values to move_base for moving my robot to the saved spots.
So, I altered my code to the following:
move_base = actionlib.SimpleActionClient('move_base_node', MoveBaseAction)
move_base.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = -2.76009410711
goal.target_pose.pose.position.y = -3.60989414856
goal.target_pose.pose.position.z = 0.0
goal.target_pose.pose.orientation.x = 0.0
goal.target_pose.pose.orientation.y = 0.0
goal.target_pose.pose.orientation.z = 0.000899836262279
goal.target_pose.pose.orientation.w = 0.999999595147
move_base.send_goal(goal)
In combination with my launch file which includes my AMCL, move_base and map_server configuration, my robot now moves.
BUT, the robot will not move into room-1 as the example asks us to attempt. It also does not move to other saved spots. I get the following error message:
Aborting because a valid control could not be found. Even after executing all recovery behaviors
Can you advice on how to resolve this problem ?
I REALLY would appreciate some help.
Thanks.
Hello @adeoduye ,
Well, the reasons for this issue can be many. I had a look at your code to find out what could be going wrong, and here are some comments:
-
First of all, you should try to make sure that your robot is properly localized on the map before trying to send any goal. You can check this with the PoseArray display in RViz (the arrows are not dispersed around the robot).
-
Second important point is the inflation of the costmaps. In your case, the inflation you are applying is too big (1m), especially for navigating through narrow spaces. From my tests, an inflation radius that worked for me was 0.32m. You can tune this parameter in the costmap_common.yaml file.
-
Finally, another issue was that your goal was too close to a wall, which was causing problems with the planner. In your script my_navigation_program_client.py, I changed the Y coordinate of the goal pose from -3.6 to -4, which sets the goal a bit more centered in the room, and then the robot was able to get to the goal.
Try to tune these parameters by yourself and find out which are the better ones.
Best,
1 Like
First, THANK YOU Alberto for your input which has helped me a lot.
If I may, I have a few follow up questtions, please.
Regarding the values set in files like costmap_common.yaml, is there a way to visualize what effects those settings would have ?
That inflation_radius for example. I just copied and used the default files as supplied in the tutorial (I think). I want to be able to deeply understand the values in all the configuration files.
You were right about the pose estimates. I had not started Rviz before. Once I started it, not only were the pose estimates all over the place and the estimated position was wrong. I set the 2D pose estimate to the correct position but running my script to move the robot into into Room-1 (the unfurnished room) failed because the robot collided with the wall.
So, I reset the simulation and drove the robot around, brought it back to it’s start position and executed my script again. This time the robot made it into the room but only after the robot spun around for a while (I have no idea why that happened).
After that, again the robot head butted the wall and it refused to move.
Is there a strategy to help the robot to reverse OR reposition so the robot can free it self ?
AND
Is there an action client which can be used to set the 2D pose estimate without using Rviz ?
Because for a pyhsical robotics project, I don’t think going into Rviz to set the pose estimate will be an option.
Hello @adeoduye ,
As I told you in my first response, I really encourage you to take the ROS Navigation course. It will answer all the questions you are making in this last post.
Best,
