How to use data of subscriber to publish and command the robot?
My main aim is to turn the robot to a specific angle. So, what I am trying to do is I have defined a subscriber and publisher in single node and then I will subscribe to the odometry data of the gazebo model through which I will calculate the yaw of the robot and until the robot gain the specific yaw I will command it a specific angular velocity. But the problem is when I define yaw in odometryCb it is not local variable so I can't use it in publisher part so tried defining variables in publisher it self but it is showing that sub has no attribute subscriber. Can anyone help me to find out how to deal it with?
#!/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_pub.csv", "w")
TEXTFILE.truncate()
def odometryCb(msg):
import csv
#csvData=[['pose_x','pose_y','twist_x','twist_z', 'Orientation' ,'time']]
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.csv', 'a') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(row)
csvFile.close()
def publisher():
pub = rospy.Publisher('robot_diff_drive_controller/cmd_vel', Twist, queue_size=10)
sub = rospy.Subscriber("robot_diff_drive_controller/odom", Odometry, odometryCb)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
cmd_vel = Twist()
odom = Odometry()
cmd_vel.linear.x=0.0
cmd_vel.angular.z=0.3
now = time.time()
du = time.time()
x = sub.subscribe(odom.pose.pose.position.x)
y = sub.subscribe(odom.pose.pose.position.y)
v_x = sub.subscribe(odom.twist.twist.linear.x)
w = sub.subscribe(odom.twist.twist.angular.z)
q0 = sub.subscribe(odom.pose.pose.orientation.w)
q1 = sub.subscribe(odom.pose.pose.orientation.x)
q2 = sub.subscribe(odom.pose.pose.orientation.y)
q3 = sub.subscribe(odom.pose.pose.orientation.z)
yaw = math.degrees(math.atan2( 2.0*(q0*q3 + q1*q2),(1.0-2.0*(q2*q2 + q3*q3))))
while(yaw < 90):
pub.publish(cmd_vel)
rate.sleep()
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
# spin() simply keeps python from exiting until this node is stopped
if __name__ == '__main__':
publisher()
Just a note:
You cannot subscribe to single fields. Only to topics.
Then, what should I be the code instead of it?
I hate to give these sort of answers, but do really try to complete the tutorials. They should show you how to do this.
Note: ROS != simulink. A topic subscription is not a signal (or it sort-of is, but then a complex one, not a single float or integer).
Actually, I found no tutorial which explains how to use data of the subscriber in the publisher of the same node. Can you provide me the link to those tutorials?
I was referring to subscribing to topics instead of trying to subscribe to individual fields.
As to publishers and subscribers in the same node: please use Google: search for:
publisher subscriber same node site:answers.ros.org
.You can do calculation and everything within the subscriber, just define publisher outside and put pub.publish int the subscriber
I tried doing that by changing the code to the following but nothing happens after a run the 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
I tried that by putting this code in subscriber. still code is not working.