Ask Your Question
0

Pointing turtlebot to the target position before moving

asked 2019-11-14 18:37:52 -0600

Satish gravatar image

I need to move my turtlebot to a specific point with the specified velocity. I don't want my turtlebot to make angular movement while it is moving, because, angular movement in between is causing additional travel time. So I am trying to set the turtle bot point towards the goal before it starts moving, but I couldn't do that. I robot is pointing to completely different direction. I am using gazibo simulation, given below is my code, please review and let me know where I need to make my changes.

 def newOdom(msg):
        global x
        global y
        global theta
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        rot_q = msg.pose.pose.orientation
        (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
rospy.init_node("RobotController")
rospy.Subscriber("/robot1/odom", Odometry, newOdom)
pub = rospy.Publisher("/robot1/cmd_vel", Twist, queue_size = 1)
speed = Twist()
r = rospy.Rate(5)

goal = Point()
goal.x = 3.3
goal.y = 3.3
distance=sqrt(pow(goal.x-x,2) + pow(goal.y-y, 2))

inc_x = goal.x-x
inc_y = goal.y-y
angle_to_goal = atan2(inc_y, inc_x)
while abs(angle_to_goal - theta) > 0.1 :
    scale=1.5
    dir=(angle_to_goal-theta) /abs( angle_to_goal-theta)
    angSpeed= min(0.5, abs( angle_to_goal-theta) / scale)
    speed.angular.z = dir*angSpeed
    speed.linear.x = 0
    pub.publish(speed)
    r.sleep()

speed.angular.z = 0
while distance >= 0.7:
    distance=sqrt(pow(goal.x-x,2) + pow(goal.y-y, 2))
    rospy.loginfo(distance)
    speed.linear.x = 0.5
    speed.angular.z = 0
    pub.publish(speed)
    r.sleep()
speed.linear.x = 0.0
speed.angular.z =0.0
pub.publish(speed)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-11-15 01:48:20 -0600

Delb gravatar image

Have you seen #q313304 ? It's the same question so you should find what you want there.

I don't want my turtlebot to make angular movement while it is moving, because, angular movement in between is causing additional travel time

I don't see why you state that, if you don't turn when reaching a goal what would happen if the orientation is not completly accurate at the beginning ? Moreover, the robot could turn a little bit while moving forward which could lead to the robot never reaching the goal.

So I would advise you to reconsider this, it's not an issue to publish angular cmd_vel while moving forward.

edit flag offensive delete link more
0

answered 2019-11-15 00:31:34 -0600

duck-development gravatar image

did you try to plot the odom and the cmd_vel ther is the rqt tools, to see waht is going inside, you may publisch the "angle_to_goal-theta"to see waht you input is

 def newOdom(msg):
        global x
        global y
        global theta
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y

        q = (msg.pose.pose.orientation.x,
              msg.pose.pose.orientation.y,
              msg.pose.pose.orientation.z,
              msg.pose.pose.orientation.w)
         euler = euler_from_quaternion(q)
         theta = euler[2]

rospy.init_node("RobotController")
rospy.Subscriber("/robot1/odom", Odometry, newOdom)
pub = rospy.Publisher("/robot1/cmd_vel", Twist, queue_size = 1)
speed = Twist()
r = rospy.Rate(5)

goal = Point()
goal.x = 3.3
goal.y = 3.3
distance=sqrt(pow(goal.x-x,2) + pow(goal.y-y, 2))

inc_x = goal.x-x
inc_y = goal.y-y
angle_to_goal = atan2(inc_y, inc_x)
while abs(angle_to_goal - theta) > 0.1 :
    scale=1.5
    dir=(angle_to_goal-theta) /abs( angle_to_goal-theta)
    angSpeed= min(0.5, abs( angle_to_goal-theta) / scale)
    speed.angular.z = dir*angSpeed
    speed.linear.x = 0
    pub.publish(speed)
    r.sleep()


speed.angular.z = 0
while distance >= 0.7:
    distance=sqrt(pow(goal.x-x,2) + pow(goal.y-y, 2))
    rospy.loginfo(distance)
    speed.linear.x = 0.5
    speed.angular.z = 0
    pub.publish(speed)
    r.sleep()
speed.linear.x = 0.0
speed.angular.z =0.0
pub.publish(speed)
edit flag offensive delete link more

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-14 18:37:52 -0600

Seen: 33 times

Last updated: Nov 15