ROS - außer Kontrolle ⇐ Python

[Anonymous]

Ich habe eine Führungsschildkröte und drei Nachfolgerschildkröten. Die Follower folgen mit der TF2 -Transformationsmethode. Gibt es eine Möglichkeit, dies zu stoppen? Manchmal kommt es korrekt und wird zum richtigen Ort, aber dies ist nicht das Verhalten, auf das ich hoffe.

Code: Select all

#!/usr/bin/env python
import rospy

import math
import tf2_ros
import geometry_msgs.msg
import turtlesim.srv
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
import time
from random import randint
from com760_assign_summer_ws_pkg.msg import bxxxxxxxxLeaderMessage

# name = ""
instruction = ""
message = ""
f2x = None

global PI, x, y, x, yaw
PI = 3.1415926535897
x=0
y=0
z=0
yaw=0
tolerance = 0.1
condition = 0

def callback(data):
global instruction, message
instruction = data.instructionID
message = data.message
# print("Instruction is: ", data.instructionID)
# return data

def msg_listener():
# global data
# print("In listener")
# rospy.Subscriber('leader_chatter', bxxxxxxxxLeaderMessage, callback)
# print(data)
pass

def msg_talker():
# print("In talker")
# sg_pub = rospy.Publisher('leader_chatter', bxxxxxxxxLeaderMessage, queue_size=10)

msg = b00830189LeaderMessage()
msg.instructionID = 0
msg.message = "Followers are to gather in formation"
msg_pub.publish(msg) # The message is cintinuously published
msg_listener()

def rotate():
#Starts a new node
# rospy.init_node('robot_cleaner', anonymous=True)
# velocity_publisher = rospy.Publisher('/leader_turtle/cmd_vel', Twist, queue_size=10)
# vel_msg = Twist()

speed = 90.0
angle = 90.0

clockwise = randint(0,1) # 0 = False, so, antilockwise, 1 - True, so, clockwise
# clockwise = False

#Converting from angles to radians
angular_speed = speed*2*PI/360
relative_angle = angle*2*PI/360

#We wont use linear components
vel.linear.x=0
vel.linear.y=0
vel.linear.z=0
vel.angular.x = 0
vel.angular.y = 0

# Checking if our movement is CW or CCW
if clockwise:
vel.angular.z = -abs(angular_speed)
else:
vel.angular.z = abs(angular_speed)
# Setting the current time for distance calculus
t0 = rospy.Time.now().to_sec()
current_angle = 0

while(current_angle <  relative_angle):
# rospy.loginfo("Rotating")
pub.publish(vel)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed*(t1-t0)

#Forcing stop
vel.angular.z = 0
pub.publish(vel)
# rospy.spin()

def poseCallback(pose_message):
global x
global y, z, yaw
x= pose_message.x
y= pose_message.y
yaw = pose_message.theta

def f1_poseCallback(f1_pose_message):
# Function to get the polition of the follower 1 turtle
global f1x, f1y, f1z, f1yaw
f1x= f1_pose_message.x
f1y= f1_pose_message.y
f1yaw = f1_pose_message.theta

def f2_poseCallback(f2_pose_message):
# Function to get the polition of the follower 2 turtle
global f2x, f2y, f2z, f2yaw
f2x= f2_pose_message.x
f2y= f2_pose_message.y
f2yaw = f2_pose_message.theta

def f3_poseCallback(f3_pose_message):
# Function to get the polition of the follower 3 turtle
global f3x, f3y, f3z, f3yaw
f3x= f3_pose_message.x
f3y= f3_pose_message.y
f3yaw = f3_pose_message.theta

def pose_callback(pose):
global robot_x
rospy.loginfo("Robot X = %f : Y = %f : Z = %f \n", pose.x, pose.y, pose.theta)
robot_x = pose.x

def straight_line_setup():
# --- Moves the turtle in a straight line after it has rotated --- #
vel.linear.x = 0.5 # Speed
vel.linear.y = 0
vel.linear.z = 0

vel.angular.x = 0
vel.angular.y = 0
vel.angular.z = 0

if __name__ == '__main__':
global vel, pub
rospy.init_node('tf2_turtle_listener')

rospy.Subscriber('leader_chatter', b00830189LeaderMessage, callback)
msg_pub = rospy.Publisher('leader_chatter', b00830189LeaderMessage, queue_size=10)

loop_rate = rospy.Rate(10) # Publishing 10Hz

turtlename = 'turtle1'
position_topic = "/turtle1/pose"
pose_subscriber = rospy.Subscriber(position_topic, Pose, poseCallback)

follow1_topic = "/turtle2/pose"
follower1_subscriber = rospy.Subscriber(follow1_topic, Pose, f1_poseCallback)
follow2_topic = "/turtle3/pose"
follower1_subscriber = rospy.Subscriber(follow2_topic, Pose, f3_poseCallback)
follow3_topic = "/turtle4/pose"
follower1_subscriber = rospy.Subscriber(follow3_topic, Pose, f3_poseCallback)

x0=x #  x0 - starts point
y0=y
distance_moved = 0.0

vel = Twist()

pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
# rospy.Subscriber('/turtle1/pose', Pose, pose_callback)

time.sleep(5) # Allowing time for everything to get set up before next thing happens

# ------- Rotating, hopefully ------ ~
rotate() # This function will go off and rotate the turtle either closewise or anticlockwise.

time.sleep(3)
# --- Moves the turtle in a straight line after it has rotated --- #
straight_line_setup() # call this instead of the commented code above

tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)

# ----------------- Spawning turtles ------------------ #
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
turtle_name_2 = rospy.get_param('turtle', 'turtle2')
# spawner(randint(2,7), randint(1,9), 0, turtle_name_2)
spawner(1, 1, 0, turtle_name_2)

rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
turtle_name_3 = rospy.get_param('turtle', 'turtle3')
# spawner(randint(7,9), randint(1,9), 0, turtle_name_3)
spawner(1, 4, 0, turtle_name_3)

rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
turtle_name_4 = rospy.get_param('turtle', 'turtle4')
spawner(1, 8, 0, turtle_name_4)

turtle_vel_2 = rospy.Publisher('%s/cmd_vel' % turtle_name_2, geometry_msgs.msg.Twist, queue_size=1)
turtle_vel_3 = rospy.Publisher('%s/cmd_vel' % turtle_name_3, geometry_msgs.msg.Twist, queue_size=1)
turtle_vel_4 = rospy.Publisher('%s/cmd_vel' % turtle_name_4, geometry_msgs.msg.Twist, queue_size=1)

rate = rospy.Rate(10.0)
while not rospy.is_shutdown():

msg_talker()

# Now, I want an if-statement.  If 0, for the followers, if 0, do the other thing
if(instruction == 0):
# pub.publish(vel) # publishing to leader turtle (i.e. main turtle)
try:
past = rospy.Time.now() - rospy.Duration(5.0)

trans2 = tfBuffer.lookup_transform(turtle_name_2, 'carrot1', rospy.Time.now(), rospy.Duration(1.0))
trans3 = tfBuffer.lookup_transform(turtle_name_3, 'carrot2', rospy.Time.now(), rospy.Duration(1.0))
trans4 = tfBuffer.lookup_transform(turtle_name_4, 'carrot3', rospy.Time.now(), rospy.Duration(1.0))
# trans3 = tfBuffer.lookup_transform('turtle3', 'carrot2', rospy.Time.now(), rospy.Duration(1.0))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
# raise
rate.sleep()
continue

msg2 = geometry_msgs.msg.Twist()
msg2.angular.z = 4 * math.atan2(trans2.transform.translation.y, trans2.transform.translation.x)
msg2.linear.x = 0.5 * math.sqrt(trans2.transform.translation.x ** 2 + trans2.transform.translation.y ** 2)

msg3 = geometry_msgs.msg.Twist()
msg3.angular.z = 4 * math.atan2(trans3.transform.translation.y, trans3.transform.translation.x)
msg3.linear.x = 0.5 * math.sqrt(trans3.transform.translation.x ** 2 + trans3.transform.translation.y ** 2)

msg4 = geometry_msgs.msg.Twist()
msg4.angular.z = 4 * math.atan2(trans4.transform.translation.y, trans4.transform.translation.x)
msg4.linear.x = 0.5 * math.sqrt(trans4.transform.translation.x ** 2 + trans4.transform.translation.y ** 2)

turtle_vel_2.publish(msg2)
turtle_vel_3.publish(msg3)
turtle_vel_4.publish(msg4)

pub.publish(vel) # Moves the leader turtle

if(((x > 10.5) or (x < 0.5)) or ((y > 10.5) or (y < 0.5))):
print("Ah!! A wall")
vel.linear.x = 0
vel.linear.y = 0
pub.publish(vel)
else:
# print("It isn't 0, you crazy fool!")
pass

rate.sleep()
< /code>
Dann habe ich auch die folgenden, in separaten Dateien. Der einzige Unterschied ist, dass der T.Child_Frame_id 

in allen drei unterschiedlich ist. Die anderen beiden haben carot2 und dann carot3 und die x- und y-locations:

Code: Select all

#!/usr/bin/env python
import rospy
import tf2_ros
import tf2_msgs.msg
import geometry_msgs.msg

class FixedTFBroadcaster:

def __init__(self):
self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)

while not rospy.is_shutdown():
# Run this loop at about 10Hz
rospy.sleep(0.1)

t = geometry_msgs.msg.TransformStamped()
t.header.frame_id = "turtle1"
t.header.stamp = rospy.Time.now()
t.child_frame_id = "carrot1"
t.transform.translation.x = -1.0
t.transform.translation.y = -1.0
t.transform.translation.z = 0.0

t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0

tfm = tf2_msgs.msg.TFMessage([t])
self.pub_tf.publish(tfm)

if __name__ == '__main__':
rospy.init_node('fixed_tf2_broadcaster')
tfb = FixedTFBroadcaster()

rospy.spin()

< /code>
Startdatei: < /p>