Ask Your Question
0

Model not rotating to commanded yaw.

asked 2018-08-30 02:17:55 -0500

Maulik_Bhatt gravatar image

I have written a publisher to rotate the robot to a specific angle but when I run that publisher robot does not rotate to that angle. Is there any fundamental mistake I am making or there is problem in ros? Is there any other way to achieve this goal? I have given the code and snapshot of the model after running that code.

#!/usr/bin/env python
import rospy 
import roslib; roslib.load_manifest('firstrobot')
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from std_msgs.msg import Header
from geometry_msgs.msg import Twist
import time
import math

TEXTFILE = open("data_publisher.csv", "w")
TEXTFILE.truncate()

 pub = rospy.Publisher('robot_diff_drive_controller/cmd_vel', Twist, queue_size=10)
 cmd_vel = Twist()
 cmd_vel.linear.x=0.0
 cmd_vel.angular.z=0.3

def odometryCb(msg):
    import csv
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    v_x = msg.twist.twist.linear.x
    w = msg.twist.twist.angular.z
    q0 = msg.pose.pose.orientation.w
    q1 = msg.pose.pose.orientation.x
    q2 = msg.pose.pose.orientation.y
    q3 = msg.pose.pose.orientation.z
    yaw = math.degrees(math.atan2(2.0*(q0*q3 + q1*q2),(1.0-2.0*(q2*q2 + q3*q3))))
    t= 1.0*msg.header.stamp.secs + 1.0*(msg.header.stamp.nsecs)/1000000000
    row = [ v_x, w, yaw, t]
    with open('data_publisher.csv', 'a') as csvFile:
        writer = csv.writer(csvFile)
        writer.writerow(row)
        csvFile.close()

    if yaw <= 90:
        pub.publish(cmd_vel)


 def listener():
        rospy.init_node('oodometry', anonymous=True)
        rospy.Subscriber("robot_diff_drive_controller/odom", Odometry, odometryCb)
        rospy.spin()

 if __name__ == '__main__':
      listener()

Model Top View after running the code above

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-08-30 04:07:12 -0500

I think I see the problem. You're expecting the robot to rotate while you publish the cmd_vel messages and to stop rotating when you stop publishing these messages?

Depending on how your robot platform is interpreting the cmd_vel topic this is not what happens. Some robots will keep moving with a velocity given by the most recent cmd_vel message until they receive a different cmd_vel message. So to be safe you want to publish a cmd_vel message with a zero angular.z value to make the robot stop rotating. Then end of your callback should therefore be:

if yaw <= 90:
    cmd_vel.angular.z = 0.3
else:
    cmd_vel.angular.z = 0.0

pub.publish(cmd_vel)

Hope this helps.

edit flag offensive delete link more

Comments

Nope actually I have tried that but that doesn't help. Robot is not even rotating up to 90%. I have subscribed to the yaw and the data coming is incorrect. yaw is showing greater angle than robot is really rotated. Please help if you know how to tackle this problem?

Maulik_Bhatt gravatar imageMaulik_Bhatt ( 2018-08-31 03:20:19 -0500 )edit

If the odom topic is incorrect that is a different question. Have you viewed odom in RVIZ to check it visually? You could then compare the yaw value you're calculating to what you see in RVIZ to verify that your equation is correct.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2018-08-31 06:25:51 -0500 )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: 2018-08-30 02:17:55 -0500

Seen: 54 times

Last updated: Aug 30 '18