def talker():
'''targetGen Publisher'''
rospy.init_node('targetGen', anonymous=True)
pub = rospy.Publisher('leader', robotState, queue_size=1)
rate = rospy.Rate(50) # 50hz
tArcSpeed = rospy.get_param("~AngleSpeed",0.1)
initArc = rospy.get_param("~initArc",0)
last_point = Point()
last_angle = 0
last_time = rospy.get_time()
last_point.x = LineFunction(initArc).x
last_point.y = LineFunction(initArc).y
start = rospy.get_time()
while start == 0:
start = rospy.get_time()
# print "start: %s" %start
while not rospy.is_shutdown():
leader = robotState()
duration = rospy.get_time() - start
if duration == 0:
continue
# print "duration: %s" %duration
angle = initArc+tArcSpeed*duration #count from x+
leader.xi = angle
leader.x = LineFunction(angle).x
leader.y = LineFunction(angle).y
leader.angSp = tArcSpeed # angler speed: given in there
leader.dirAngle = math.atan2(leader.y - last_point.y, leader.x - last_point.x)
last_point.x = leader.x
last_point.y = leader.y
curTime = rospy.get_time()
if last_time == curTime:
continue
# print "leaderTime:%s; (%s, %s); xi: %s; anSp: %s; dir: %s" % (duration, leader.x, leader.y, leader.xi, leader.angSp, leader.dirAngle)
RobotAnSpee = (leader.dirAngle - last_angle) / (curTime - last_time)
last_time = curTime
last_angle =leader.dirAngle
#pub leader messages
pub.publish(leader)
rate.sleep()
#!/usr/bin/env python
import rospy
import sys
from geometry_msgs.msg import Twist, Point
import math
from target.msg import robotState
import tf
import numpy as np
leaderInfo = robotState()
CurRobot = robotState()
Robot1 = robotState()
Robot2 = robotState()
Robot3 = robotState()
Robot4 = robotState()
def getLeader(data):
global leaderInfo
leaderInfo = data
def getOwnMes0(data):
global CurRobot
CurRobot = data
def getOwnMes1(data):
global Robot1
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
import math
from target.msg import robotState
import tf
import numpy as np
ns = '/robot_1'
leaderInfo = robotState()
CurRobot = robotState()
rho = 0.5
def getLeader(data):
global leaderInfo
leaderInfo = data
def getOwnMes(data):
global CurRobot
CurRobot = data
class followTest():
def __init__(self):
#!/usr/bin/env python
# this is used for publish some need formation
# pub message 50 hz
'''dd ROS Node'''
import rospy
from std_msgs.msg import String
from target.msg import robotState
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
import tf
import math
import numpy as np
curRobotPos = robotState()
last_angle_global = 0
elapse_angle_global = 0
last_time = 0
last_pos = Point()
rho = 0
pub = rospy
def getName():
global name
global pub
global rho
name = rospy.get_param("~number", "robot_1")
pub = rospy.Publisher("/" + name + "/robotStates",
robotState,