ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to use data of subscriber to publish and command the robot?

asked 2018-08-26 03:58:53 -0500

Maulik_Bhatt gravatar image

updated 2018-08-26 04:02:36 -0500

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()
edit retag flag offensive close merge delete

Comments

Just a note:

x = sub.subscribe(odom.pose.pose.position.x)

You cannot subscribe to single fields. Only to topics.

gvdhoorn gravatar image gvdhoorn  ( 2018-08-26 04:29:28 -0500 )edit

Then, what should I be the code instead of it?

Maulik_Bhatt gravatar image Maulik_Bhatt  ( 2018-08-26 04:37:03 -0500 )edit

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).

gvdhoorn gravatar image gvdhoorn  ( 2018-08-26 04:37:48 -0500 )edit

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?

Maulik_Bhatt gravatar image Maulik_Bhatt  ( 2018-08-26 04:43:56 -0500 )edit

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.

gvdhoorn gravatar image gvdhoorn  ( 2018-08-26 05:20:34 -0500 )edit

You can do calculation and everything within the subscriber, just define publisher outside and put pub.publish int the subscriber

Choco93 gravatar image Choco93  ( 2018-08-27 02:28:54 -0500 )edit

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

Maulik_Bhatt gravatar image Maulik_Bhatt  ( 2018-08-27 03:18:31 -0500 )edit

I tried that by putting this code in subscriber. still code is not working.

rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10)
cmd_vel = Twist()
cmd_vel.linear.x=0.0
cmd_vel.angular.z=0.3
now = time.time()
while(yaw < 90):
pub.publish(cmd_vel)
Maulik_Bhatt gravatar image Maulik_Bhatt  ( 2018-08-27 03:22:09 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-08-30 02:29:06 -0500

Maulik_Bhatt gravatar image

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()
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2018-08-26 03:58:53 -0500

Seen: 3,186 times

Last updated: Aug 30 '18