Hello,
I'm working on two pioneer 3dx, and i'm trying to make one follow the other. To do that I must first publish a cmd_vel/Twist to my master robot and then publish another cmd_vel/Twist to my slave (based on the space between the robots).
The scripts work fine on their own but how can I publish to my master while publishing to my slave since both are using a transform on "twist.linear.x" ?
I wrote this script for the slave :
#!/usr/bin/env python
import getch
import roslib; roslib.load_manifest('p3dx_mover')
import sys
import rospy
import math
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Range
#########Publisher
pub = rospy.Publisher('/r1/cmd_vel', Twist)
rospy.init_node('p3dx_mover')
twist = Twist()
########Variables
rate = rospy.Rate(rospy.get_param('~hz', 10))
s_min = 0.3 #minimum space between slave & master
s_max = 2 #maximum space between slave & master
v_max = 0.4 #maximum velocity of slave
alpha = v_max / (s_max - s_min) #slope of the velocity function
b = alpha*s_max - v_max #intercept of the velocity function
#########Subscriber
def callback(msg):
rospy.loginfo("Sonar 4 detects something at %s m" % round(msg.range,3))
if msg.range < 0.22:
rospy.loginfo("Obstacle too close ! Not normal, vehicule stopped (You might want to revise the code)")
twist.linear.x = 0
pub.publish(twist)
rospy.signal_shutdown("Simulation end")
beta = alpha*msg.range - b
gamma = min(beta,v_max)
twist.linear.x = max(0, gamma)
sub = rospy.Subscriber("/r1/sonar4/scan", Range, callback)
#########Loop
while not rospy.is_shutdown() :
rospy.loginfo("Velocity is "+ str(round(twist.linear.x,3))+ " m/s" )
pub.publish(twist)
rate.sleep()
And for the master a simple one such as :
#!/usr/bin/env python
import getch
import roslib; roslib.load_manifest('p3dx_mover')
import sys
import rospy
import math
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Range
#########Publisher
pub = rospy.Publisher('/r2/cmd_vel', Twist)
rospy.init_node('p3dx_mover')
twist = Twist()
########Variables
twist.linear.x = 0
i=0
rate = rospy.Rate(rospy.get_param('~hz', 10))
#########Loop
while not rospy.is_shutdown() :
twist.linear.x = (math.cos(i) +2)/8
i+=0.1
rospy.loginfo("Velocity is "+ str(round(twist.linear.x,3))+ " m/s" )
pub.publish(twist)
rate.sleep()
While looking at multiple ros python code, I saw that many use classes and functions to do these sort of things. I tried but since i'm not at ease with these things, nothing worked.
If anyone could help me, feel free to contribute ! If more informations are needed, I remain at your disposal.
Hdifsttar
↧
