Robotics StackExchange | Archived questions

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

Comments

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.

  1. 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.
  2. 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