Revision history [back]

I finally got my code working with the code bellow.

#!/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()