def to_obsarray():
array = ObstacleArrayMsg()
array.header.frame_id = 'field'
oid = [0]
def next_id():
oid[0] += 1
return oid[0] - 1
red_line = [to_point((0, 4.1)), to_point((1.9, 6))]
blue_line = [to_point((4.1, 0)), to_point((6, 1.9))]
for l in (blue_line, red_line):
obs = ObstacleMsg(polygon=Polygon(l))
obs.id = next_id()
array.obstacles.append(obs)
for stationary in ((2, 4), (4, 2)):
obs = ObstacleMsg(polygon=to_square(stationary, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for color, pos in world.goals:
obs = ObstacleMsg(polygon=to_square(pos, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
for pos in world.cones:
obs = ObstacleMsg(polygon=to_square(pos, 0.076))
obs.id = next_id()
array.obstacles.append(obs)
return array
def publish_obstacle_msg(self):
if self.args.no_ros:
return
else:
from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import Point32, Quaternion
from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = self.get_sim_time()
obstacles_msg.header.frame_id = kFixedFrame
for i in range(1, self.n_sim_agents):
# Add point obstacle
obst = ObstacleMsg()
obst.id = i
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = self.rlenv.virtual_peppers[i].pos[0]
obst.polygon.points[0].y = self.rlenv.virtual_peppers[i].pos[1]
obst.polygon.points[0].z = 0
obst.radius = self.rlenv.vp_radii[i]
yaw = self.rlenv.virtual_peppers[i].pos[2]
q = quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = self.rlenv.virtual_peppers[i].vel[0]
obst.velocities.twist.linear.y = self.rlenv.virtual_peppers[i].vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = self.rlenv.virtual_peppers[i].vel[2]
obstacles_msg.obstacles.append(obst)
self.obstpub.publish(obstacles_msg)
return
def to_obsarray_bulked():
array = ObstacleArrayMsg()
array.header.frame_id = 'field'
oid = [0]
def next_id():
oid[0] += 1
return oid[0] - 1
lines = [
# small cone corner
((0, 0), (0, 2.125)),
((0, 0), (2.125, 0)),
((1, 0), (1, 2.125)),
((0, 1), (2.125, 1)),
# long cone corner
((6, 6), (6, 6 - 3.125)),
((6, 6), (6 - 3.125, 6)),
((5, 6), (5, 6 - 3.125)),
((6, 5), (6 - 3.125, 5)),
( # center cones
(2, 2), (2, 2.5), (3.5, 4), (4, 4), (4, 3.5), (2.5, 2)),
# goal line
((0, 4.1), (1.9, 6)),
((4.1, 0), (6, 1.9))
]
for l in lines:
obs = ObstacleMsg(polygon=Polygon([to_point(p) for p in l]))
obs.id = next_id()
array.obstacles.append(obs)
for stationary in ((2, 4), (4, 2)):
obs = ObstacleMsg(polygon=to_square(stationary, 0.127))
obs.id = next_id()
array.obstacles.append(obs)
return array
def publish_obstacle_msg(self):
print("Testing_obstacle_msg")
#obstacle_list = [ObstacleMsg()]
obstacle_arr_msg = ObstacleArrayMsg()
obstacle_arr_msg.header.stamp = rospy.Time.now()
obstacle_arr_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map
# Add point obstacle
obstacle_msg = ObstacleMsg()
obstacle_msg.header.stamp = rospy.Time.now()
obstacle_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map
obstacle_msg.radius = 0.5
obstacle_msg.id = 2
obstacle_msg.polygon.points = [Point32()]
obstacle_msg.polygon.points[0].x = self.rob_1_pose_trans[0]
obstacle_msg.polygon.points[0].y = self.rob_1_pose_trans[1]
obstacle_msg.polygon.points[0].z = 0
obstacle_msg.orientation.x = self.rob_1_pose_rot[0]
obstacle_msg.orientation.y = self.rob_1_pose_rot[1]
obstacle_msg.orientation.z = self.rob_1_pose_rot[2]
obstacle_msg.orientation.w = self.rob_1_pose_rot[3]
obstacle_msg.velocities.twist.linear.x = self.vel_x
obstacle_msg.velocities.twist.linear.y = self.vel_y
obstacle_msg.velocities.twist.linear.z = 0
obstacle_msg.velocities.twist.angular.x = 0
obstacle_msg.velocities.twist.angular.y = 0
obstacle_msg.velocities.twist.angular.z = 0
obstacle_arr_msg.obstacles.append(obstacle_msg)
#print(obstacle_arr_msg)
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.rob_2_pub.publish(obstacle_arr_msg)
r.sleep()
def callback_base_pose_ground_truth(base_pose_ground_truth, obst_id):
# Search for obstacle id and update fields if found
if obst_id == 10: return # skip the Ball
i = -1 # -1 is the last element
for idx, obst in enumerate(obstacles_msg.obstacles):
if obst.id == obst_id:
i = idx
if i == -1:
obstacle_msg = ObstacleMsg()
obstacle_msg.id = obst_id
obstacles_msg.obstacles.append(obstacle_msg)
# HEADER
obstacles_msg.header.stamp = rospy.Time.now()
obstacles_msg.header.frame_id = 'map'
# OBSTACLES
obstacles_msg.obstacles[i].header.stamp = obstacles_msg.header.stamp
obstacles_msg.obstacles[i].header.frame_id = obstacles_msg.header.frame_id
obstacles_msg.obstacles[i].polygon.points = [
Point32()
] # currently only point obstacles
obstacles_msg.obstacles[i].polygon.points[
0].x = base_pose_ground_truth.pose.pose.position.x
obstacles_msg.obstacles[i].polygon.points[
0].y = base_pose_ground_truth.pose.pose.position.y
obstacles_msg.obstacles[i].polygon.points[
0].z = base_pose_ground_truth.pose.pose.position.z
# ORIENTATIONS
#yaw = math.atan2(base_pose_ground_truth.twist.twist.linear.y, base_pose_ground_truth.twist.twist.linear.x)
#quat = quaternion_from_euler(0.0, 0.0, yaw) # roll, pitch, yaw in radians
#obstacles_msg.obstacles[i].orientation = Quaternion(*quat.tolist())
obstacles_msg.obstacles[
i].orientation = base_pose_ground_truth.pose.pose.orientation
# VELOCITIES
obstacles_msg.obstacles[i].velocities = base_pose_ground_truth.twist
def add_obstacle(self, id, posx, posy, vel_x, vel_y, range_x, range_y):
self.pose_x.append(posx)
self.pose_y.append(posy)
self.range_x.append(range_x)
self.range_y.append(range_y)
samp_obstacle = ObstacleMsg()
samp_obstacle.id = id
samp_obstacle.radius = 0.35
samp_obstacle.polygon.points = [Point32()]
samp_obstacle.polygon.points[0].x = posx
samp_obstacle.polygon.points[0].y = posy
samp_obstacle.polygon.points[0].z = 0
yaw = math.atan2(vel_y, vel_x)
q = tf.transformations.quaternion_from_euler(0, 0, yaw)
samp_obstacle.orientation = Quaternion(*q)
samp_obstacle.velocities.twist.linear.x = vel_x
samp_obstacle.velocities.twist.linear.y = vel_y
samp_obstacle.velocities.twist.linear.z = 0
samp_obstacle.velocities.twist.angular.x = 0
samp_obstacle.velocities.twist.angular.y = 0
samp_obstacle.velocities.twist.angular.z = 0
return samp_obstacle
def publish_obstacles(self):
with self.lock:
if self.transport_data is None:
return
# non-leg obstacles
timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data
# tracks
track_ids = []
tracks_latest_pos, tracks_color = [], []
tracks_in_frame, tracks_velocities = [], []
tracks_radii = []
for trackid in self.tracker.active_tracks:
track = self.tracker.active_tracks[trackid]
xy = np.array(track.pos_history[-1])
is_track_in_frame = True
if trackid in self.tracker.latest_matches:
color = (0.,1.,0.,1.) # green
elif trackid in self.tracker.new_tracks:
color = (0.,0.,1.,1.) # blue
else:
color = (0.7, 0.7, 0.7, 1.)
is_track_in_frame = False
track_ids.append(trackid)
tracks_latest_pos.append(xy)
tracks_color.append(color)
tracks_in_frame.append(is_track_in_frame)
tracks_velocities.append(track.estimate_velocity())
tracks_radii.append(track.avg_radius())
# publish trackedpersons
from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson
pub = rospy.Publisher("/tracked_persons", TrackedPersons, queue_size=1)
tp_msg = TrackedPersons()
tp_msg.header.frame_id = self.kFixedFrame
tp_msg.header.stamp = timestamp
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
# if not in_frame:
# continue
tp = TrackedPerson()
tp.track_id = trackid
tp.is_occluded = False
tp.is_matched = in_frame
tp.detection_id = trackid
tp.pose.pose.position.x = xy[0]
tp.pose.pose.position.y = xy[1]
heading_angle = np.arctan2(vel[1], vel[0]) # guess heading from velocity
from geometry_msgs.msg import Quaternion
tp.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, heading_angle))
tp.twist.twist.linear.x = vel[0]
tp.twist.twist.linear.y = vel[1]
tp.twist.twist.angular.z = 0 # unknown
tp_msg.tracks.append(tp)
pub.publish(tp_msg)
pub = rospy.Publisher('/obstacles', ObstacleArrayMsg, queue_size=1)
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii):
if not in_frame:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = trackid
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = xy[0]
obst.polygon.points[0].y = xy[1]
obst.polygon.points[0].z = 0
obst.radius = radius
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = vel[0]
obst.velocities.twist.linear.y = vel[1]
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/close_nonleg_obstacles', ObstacleArrayMsg, queue_size=1)
MAX_DIST_STATIC_CLUSTERS_M = 3.
cogs_in_fix = []
for i in range(len(cogs)):
cogs_in_fix.append(apply_tf(cogs[i], Pose2D(tf_rob_in_fix)))
obstacles_msg = ObstacleArrayMsg()
obstacles_msg.header.stamp = timestamp
obstacles_msg.header.frame_id = self.kFixedFrame
for c, cog_in_fix, cog_in_rob, r, is_leg in zip(clusters, cogs_in_fix, cogs, radii, is_legs):
if np.linalg.norm(cog_in_rob) < self.kRobotRadius:
continue
if len(c) < self.kMinClusterSize:
continue
# leg obstacles are already published in the tracked obstacles topic
if is_leg:
continue
# close obstacles only
if np.linalg.norm(cog_in_rob) > MAX_DIST_STATIC_CLUSTERS_M:
continue
# Add point obstacle
obst = ObstacleMsg()
obst.id = 0
obst.polygon.points = [Point32()]
obst.polygon.points[0].x = cog_in_fix[0]
obst.polygon.points[0].y = cog_in_fix[1]
obst.polygon.points[0].z = 0
obst.radius = r
yaw = 0
q = tf.transformations.quaternion_from_euler(0,0,yaw)
from geometry_msgs.msg import Quaternion
obst.orientation = Quaternion(*q)
obst.velocities.twist.linear.x = 0
obst.velocities.twist.linear.y = 0
obst.velocities.twist.linear.z = 0
obst.velocities.twist.angular.x = 0
obst.velocities.twist.angular.y = 0
obst.velocities.twist.angular.z = 0
obstacles_msg.obstacles.append(obst)
pub.publish(obstacles_msg)
pub = rospy.Publisher('/obstacle_markers', MarkerArray, queue_size=1)
# delete all markers
ma = MarkerArray()
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "obstacles"
mk.id = 0
mk.type = 0 # ARROW
mk.action = 3 # deleteall
ma.markers.append(mk)
pub.publish(ma)
# publish tracks
ma = MarkerArray()
id_ = 0
# track endpoint
for trackid, xy, in_frame, vel in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities):
if not in_frame:
continue
normvel = np.linalg.norm(vel)
if normvel == 0:
continue
mk = Marker()
mk.header.frame_id = self.kFixedFrame
mk.ns = "tracks"
mk.id = trackid
mk.type = 0 # ARROW
mk.action = 0
mk.scale.x = np.linalg.norm(vel)
mk.scale.y = 0.1
mk.scale.z = 0.1
mk.color.r = color[0]
mk.color.g = color[1]
mk.color.b = color[2]
mk.color.a = color[3]
mk.frame_locked = True
mk.pose.position.x = xy[0]
mk.pose.position.y = xy[1]
mk.pose.position.z = 0.03
yaw = np.arctan2(vel[1], vel[0])
q = tf.transformations.quaternion_from_euler(0,0,yaw)
mk.pose.orientation = Quaternion(*q)
ma.markers.append(mk)
pub.publish(ma)