def GetStates(I):
global states_in,quad_est,flag,pen_est
# check to see if cortex is frozen
Equal = StatesEqual(states_in, I.Obj[0])
if not Equal:
states_in = I.Obj[0]
if I.obj[0].visible and I.obj[1].visible
pen_vis = True
else
pen_vis = False
if flag == 0 and I.Obj[0].visible: # initialize estimate
rospy.loginfo("%s's state initialized",I.Obj[0].name)
quad_est[0] = I.Obj[0].x
quad_est[1] = I.Obj[0].y
quad_est[2] = I.Obj[0].z
quad_est[3] = I.Obj[0].u
quad_est[4] = I.Obj[0].v
quad_est[5] = I.Obj[0].w
quad_est[6] = I.Obj[0].phi*np.pi/180
quad_est[7] = I.Obj[0].theta*np.pi/180
quad_est[8] = I.Obj[0].psi*np.pi/180
quad_est[9] = I.Obj[0].p*np.pi/180
quad_est[10] = I.Obj[0].q*np.pi/180
quad_est[11] = I.Obj[0].r*np.pi/180
pen_est[0] = I.Obj[1].x - quad_est[0]
pen_est[1] = I.Obj[1].y - quad_est[1]
pen_est[2] = I.Obj[1].u - quad_est[3]
pen_est[3] = I.Obj[1].v - quad_est[4]
flag = 1
elif Equal:
pass # redundant data (do not use)
else:
rospy.log("strange stuff")
def waitForRandomTime(self, wait_type='searching'):
"""
Divide the maneuver time in slots of 20 seconds
so a duckiebot has to wait a random number of slots before performing
the maneuver. The Duckiebot that is going around the parking area searching
for parking, stops only if it knows that the one who want to exit can do the
maneuver in the next 5 sec (time needed for the maneuver).
"""
secs = time.localtime().tm_sec
slot = secs // self.time_slot_exiting
delta = (slot + 1) * self.time_slot_exiting - secs
rospy.log('Waiting a random time')
if wait_type == 'exiting':
wait = random.randrange(1, 4) * self.time_slot_exiting + delta
self.log('Wait before exiting')
self.pauseOperations(wait)
else:
slot_search = delta // self.time_slot_searching
# stop only if it is in the first or in the last time slot,
# because the car can go out
if slot_search == 0 or slot_search == 3:
self.pauseOperations(self.time_slot_searching)
self.log('Wait before going straight')
else:
self.log('Can go straight')
def cbEStop(self, msg):
"""Callback that enables/disables emergency stop
Args:
msg (BoolStamped): emergency_stop flag
"""
self.estop = msg.data
if self.estop:
rospy.log("Emergency Stop Activated")
else:
rospy.log("Emergency Stop Released")
def sonar_callback(data):
robot_obstacle.front_sensor[0] = data.front_0;
robot_obstacle.front_sensor[1] = data.front_1;
robot_obstacle.front_sensor[2] = data.front_2;
robot_obstacle.front_sensor[3] = data.front_3;
robot_obstacle.back_sensor[0] = data.back_0;
robot_obstacle.back_sensor[1] = data.back_1;
robot_obstacle.back_sensor[2] = data.back_2;
robot_obstacle.back_sensor[3] = data.back_3;
if (robot_drive.show_log):
rospy.log("f: %d, %d, %d, %d, b: %d, %d, %d, %d", data.front_0, data.front_1, data.front_2,data.front_3, data.back_0, data.back_1, data.back_2, data.back_3)
return;
def move_square(side, reps):
try:
for i in range(4 * reps):
robot.move_forward_dist(target_dist=side, use_accel=True)
robot.stop()
rospy.sleep(1)
robot.set_start_pose()
print(robot.max_ang_vel)
robot.turn_cw(ang_vel=0.5)
print(radians(45) / robot.max_ang_vel)
rospy.sleep(radians(45) /
robot.max_ang_vel) #sleep to turn 90 (45??!?) deg
robot.stop()
rospy.sleep(1)
robot.set_start_pose()
rospy.sleep(1)
return True
except Exception as err:
rospy.log(err)
return False
def main():
argv = rospy.myargv(argv=sys.argv)[1:]
rospy.init_node("state_publisher")
log('State publisher started')
send_map_transform()
log('Sent map frame.')
global broadcaster
broadcaster = tf2_ros.TransformBroadcaster()
for turtle in argv:
log(f'Started publishing transform for {TURTLE_FRAME(turtle)}.')
t = tf2_ros.TransformStamped()
t.header.frame_id = BASE_FRAME
t.child_frame_id = TURTLE_FRAME(turtle)
t.transform.translation.z = 0
t.transform.rotation.x = 0
t.transform.rotation.y = 0
rospy.Subscriber(f'/{turtle}/pose', Pose, publish_transform, t)
rospy.spin()
def main():
argv = rospy.myargv(argv=sys.argv)[1:]
rospy.init_node("state_publisher")
log('Follower started.')
if len(argv) != 2:
rospy.logerr("Invalid arguments.")
return 1
turtle, goal = argv
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
publisher = rospy.Publisher(TURTLE_CMD_VEL(turtle), Twist, queue_size=1)
vel = Twist()
rate = rospy.Rate(60)
while not rospy.is_shutdown():
try:
t = tfBuffer.lookup_transform(TURTLE_FRAME(turtle), TURTLE_FRAME(goal), rospy.Time()).transform
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rate.sleep()
continue
if t.translation.x ** 2 + t.translation.y ** 2 > MIN_DIST_SQ:
# TODO Change this thing.
theta = atan2(t.translation.y, t.translation.x)
vr = 1 + 2 * sin(theta / 2)
vl = 1 - 2 * sin(theta / 2)
vel.linear.x = VEL_MAX * (vr + vl) / 2
vel.angular.z = (vr - vl) / 2 * pi
else:
vel.linear.x = 0
vel.angular.z = 0
publisher.publish(vel)
rate.sleep()
def sensor_callback(sensor_cmd):
rospy.log("Sensor call received for %3.f seconds",sensor_cmd.sensing_time)
def on_shutdown(self):
self.publish_command(0, 0)
rospy.log('Shutting down.')
def destroySubscribers(self):
self.sub_indoors_topic.unregister()
self.sub_outdoors_topic.unregister()
log("Hope you enjoyed our service! Come back any time!")
def callback(msg):
rospy.log("starting callback")
#print len(msg.ranges)
range_ahead = msg.ranges[len(msg.ranges) / 2]
print(range_ahead)
def receive(com):
rospy.log("Receiving from microcontroller")
data = '' #where the received data will be stored
startCharsCount = 0 #we have to receive ^^^ to know the packet has started
dataLength = 0 #immediately following ^^^ will be a character representing how many bytes of data we will receive
com.open() #do I really need to do this?
pac = packet()
while True:
if start_chars_count < 3: #waiting for start bits
#TODO: Implement timeout
if com.read(1) == '^':
startCharsCount += 1
rospy.log("Received start char %i of 3", startCharsCount)
else: #we're getting a packet! I'm preserving the packet structure from 2014, it seemed solid
pac.error = 0
dataLength = ord(
com.read(1)
) #nifty trick I totally stole from Forrest, turns string character into it's unicode representation
pac.msgLength = dataLength
rospy.log("Receiving %i bytes from microcontroller", dataLength)
data = com.read(dataLength) #receive dataLength bytes
pac.timeStamp = rospy.Time.now().nsecs
pac.msgType = data[0]
rospy.log("we received a packet of type %s", pac.msgType)
pac.msgData = data[1:]
rospy.log("Pac body: %s", pac.msgData)
if pac.msgData.length != pac.msgLength:
pac.error = 1
rospy.log("Error reading packet")
com.close() #Do I really need to do this?
return pac
def testReceive(come): #just to test receiving bytes
rospy.log("Testing receiving from microcontroller")
while True:
rospy.log("%s", com.read(1))
