sending to a certain position in space using Twist
I am using the human following with OpenCV and I'd like to move the robot to the last observed position. I tried to use move_base
which relies on actionlib
and map
but unfortunately, it is not stable with OpenCV.
it shows many warn messages and its movement is uncontrollable.
full code:
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
class GoToPose():
def __init__(self):
self.goal_sent = False
rospy.on_shutdown(self.shutdown)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Wait for the action server to come up")
self.move_base.wait_for_server(rospy.Duration(5))
def goto(self, pos, quat):
self.goal_sent = True
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'base_link'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))
self.move_base.send_goal(goal)
success = self.move_base.wait_for_result(rospy.Duration(60))
state = self.move_base.get_state()
result = False
if success and state == GoalStatus.SUCCEEDED:
# We made it!
result = True
else:
self.move_base.cancel_goal()
self.goal_sent = False
return result
def shutdown(self):
if self.goal_sent:
self.move_base.cancel_goal()
rospy.loginfo("Stop")
rospy.sleep(1)
if __name__ == '__main__':
try:
rospy.init_node('nav_test', anonymous=False)
navigator = GoToPose()
# Customize the following values so they are appropriate for your location
position = {'x': 1, 'y' : 0.0}
quaternion = {'r1': 0.000, 'r2': 0.000, 'r3': 0.000, 'r4': 1.000}
rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
success = navigator.goto(position, quaternion)
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
except rospy.ROSInterruptException:
rospy.loginfo("Ctrl-C caught. Quitting")
So, I'd like to use Twist
instead of move_base
and keep 'base_link'
as the main reference instead of Odom
.
I got this code here . but it uses Odom
as a reference.
please any idea or suggestions, it would be appreciated.
Asked by Redhwan on 2019-11-29 01:41:49 UTC
Answers
All the warnings in move_base are there for a reason, they can tell you if there are any mismatches that shouldn't be there. But if you don't want to use it, then you have few options.
- Take a look at demo_pioneer. It tracks your object based on a pose. You can provide that pose to tracker node and it will do the rest for you.
- Take a local planner from nav_stack and modify it to work with your setup.
NOTE: Odom is important to have because it gives you sense of how much you have moved and will tell you when you have reached your position. In your case if your tracked human is not visible anymore getting robot to exact position of where it saw human last will be troublesome as you are not getting any new pose information.
Asked by Choco93 on 2019-11-29 03:01:02 UTC
Comments
first of all, thanks for your help.
for the first option, the robot follows the target and estimates the position of the target (x,y)
at the same time.
all of the needed, the robot moves to the last position when it loses the target and stops.
Asked by Redhwan on 2019-11-29 03:33:56 UTC
You will have to implement that yourself I guess. Shouldn't be very difficult, I did something similar long time ago. So when you receive a zero pose, you just try to move it to the last pose and based on your speed your change pose. And glad I could hep.
Asked by Choco93 on 2019-11-29 04:09:48 UTC
And do mark answer as correct if you think this helps.
Asked by Choco93 on 2019-11-29 04:52:47 UTC
Thank you again to try to help me @Choco93.
Sorry, I late to reply.
definitely, if your answer will help me to fix the issue, I will mark it.
I am not working on it now but maybe tomorrow.
Asked by Redhwan on 2019-11-30 07:00:27 UTC
Comments