Ask Your Question
0

sending to a certain position in space using Twist

asked 2019-11-29 00:41:49 -0600

Redhwan gravatar image

updated 2019-11-29 00:43:45 -0600

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-11-29 02:01:02 -0600

Choco93 gravatar image

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.

edit flag offensive delete link more

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.

Redhwan gravatar imageRedhwan ( 2019-11-29 02:33:56 -0600 )edit

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.

Choco93 gravatar imageChoco93 ( 2019-11-29 03:09:48 -0600 )edit

And do mark answer as correct if you think this helps.

Choco93 gravatar imageChoco93 ( 2019-11-29 03:52:47 -0600 )edit

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.

Redhwan gravatar imageRedhwan ( 2019-11-30 06:00:27 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-11-29 00:41:49 -0600

Seen: 20 times

Last updated: Nov 29