Python robotState Examples

Example #1

File: targetGen_pub.py Project: zk88221213/my_experiment_MAS

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

Example #2

File: follower_first.py Project: zk88221213/my_experiment_MAS

#!/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

Example #3

File: follower_second.py Project: zk88221213/my_experiment_MAS

#!/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):

Example #4

File: odomToStates.py Project: zk88221213/my_experiment_MAS

#!/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,