3 import rospy, math, random, re
4 from tf.transformations
import quaternion_from_euler
5 from geometry_msgs.msg
import Twist, PolygonStamped, Quaternion, QuaternionStamped, TwistWithCovariance, Point32
6 from nav_msgs.msg
import Odometry
7 from costmap_converter.msg
import ObstacleArrayMsg, ObstacleMsg
10 obstacles_msg = ObstacleArrayMsg()
11 obst_pub = rospy.Publisher(
'/robot_0/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size = 10)
16 for idx, obst
in enumerate(obstacles_msg.obstacles):
17 if obst.id == obst_id:
21 obstacle_msg = ObstacleMsg()
22 obstacle_msg.id = obst_id
23 obstacles_msg.obstacles.append(obstacle_msg)
26 obstacles_msg.header.stamp = rospy.Time.now()
27 obstacles_msg.header.frame_id =
'map' 30 obstacles_msg.obstacles[i].header.stamp = obstacles_msg.header.stamp
31 obstacles_msg.obstacles[i].header.frame_id = obstacles_msg.header.frame_id
32 obstacles_msg.obstacles[i].polygon.points = [Point32()]
33 obstacles_msg.obstacles[i].polygon.points[0].x = base_pose_ground_truth.pose.pose.position.x
34 obstacles_msg.obstacles[i].polygon.points[0].y = base_pose_ground_truth.pose.pose.position.y
35 obstacles_msg.obstacles[i].polygon.points[0].z = base_pose_ground_truth.pose.pose.position.z
38 yaw = math.atan2(base_pose_ground_truth.twist.twist.linear.y, base_pose_ground_truth.twist.twist.linear.x)
39 quat = quaternion_from_euler(0.0, 0.0, yaw)
40 obstacles_msg.obstacles[i].orientation = Quaternion(*quat.tolist())
43 obstacles_msg.obstacles[i].velocities = base_pose_ground_truth.twist
47 rospy.init_node(
"GroundTruthObstacles")
50 published_topics, published_types = zip(*rospy.get_published_topics())
51 r = re.compile(
"^\/robot_([1-9][0-9]*)\/base_pose_ground_truth$")
52 bpgt_topics = filter(r.match, published_topics)
54 for topic
in bpgt_topics:
55 match = r.search(topic)
56 obst_id = int(match.group(1))
57 sub = rospy.Subscriber(topic, Odometry, callback_base_pose_ground_truth, obst_id)
61 while not rospy.is_shutdown():
63 obst_pub.publish(obstacles_msg)
67 if __name__ ==
'__main__':
70 except rospy.ROSInterruptException:
def callback_base_pose_ground_truth(base_pose_ground_truth, obst_id)