def onReconfigure(self, worlds_names): """ """ #rospy.loginfo("reconfigure") text = OverlayText() text.action = 1 self.text_pub.publish(text) pass
def onReconfigure(self, worlds_names): """ """ text = OverlayText() text.action = 1 for world_name in worlds_names: if world_name not in self.__text_pub: self.__text_pub[world_name] = rospy.Publisher(world_name + "/timeline", OverlayText, queue_size=2) self.__text_pub[world_name].publish(text) pass
def update_msg(self, topic, data): self._lock.acquire() if isinstance(data, str): t = OverlayText msg = OverlayText() msg.action = OverlayText.ADD msg.text = str(data) elif isinstance(data, Real): t = Float32 msg = Float32() msg.data = float(data) else: raise ValueError("invalid msg type") if not topic in self._pubs: self._pubs[topic] = PublisherWrapper(topic, t, self._publish_rate, self._queue_size) self._pubs[topic].update_msg(msg) self._lock.release()
def odometry_callback(odometry): # speed info twist = odometry.twist.twist speed = magnitude(twist.linear) msg = Float32() msg.data = speed linear_speed_pub.publish(msg) # position info pos = odometry.pose.pose.position position_text = OverlayText() position_text.action = OverlayText.ADD position_text.width = 128 position_text.height = 64 position_text.left = 0 position_text.top = 0 position_text.bg_color = ColorRGBA(0, 0, 0, 0.5) position_text.fg_color = ColorRGBA(1, 1, 1, 1) position_text.text_size = 8 position_text.text = "Current position: {:.2f} {:.2f} {:.2f}".format(pos.x, pos.y, pos.z) position_pub.publish(position_text)
def trajectory_callback(traj): id_gen = id_generator(1) # path visualizing # waypoint position, connected with line traj_viz = MarkerArray() path_viz = Marker() path_viz.header.frame_id = 'world' path_viz.header.stamp = rospy.Time.now() path_viz.id = next(id_gen) path_viz.scale = Vector3(0.05, 0, 0) path_viz.color = ColorRGBA(0, 1, 0, 1) path_viz.type = Marker.LINE_STRIP path_viz.action = Marker.ADD for pt in traj.points: path_viz.points.append(pt.transforms[0].translation) traj_viz.markers.append(path_viz) # waypoint direction. (linear velocity at waypoint) # the direction of linear velocity is the same as the x direction of the drone, which is also the # desired heading of the drone. # # to fully determine the orientation, we still need the direction of y or z axis of the drone # after that we can construct a rotation matrix, and then convert it into quaternion num_points = len(traj.points) num_velocity_viz = 5 step = max(1, num_points / num_velocity_viz) i = 0 while i < num_points: pt = traj.points[i] translation = pt.transforms[0].translation linear = pt.velocities[0].linear norm = math.sqrt(linear.x ** 2 + linear.y ** 2 + linear.z ** 2) if norm < 1e-3: i += step continue q = geo.vector_to_quat(linear) velocity_viz = Marker() velocity_viz.header.frame_id = "world" velocity_viz.header.stamp = rospy.Time.now() velocity_viz.id = next(id_gen) velocity_viz.type = Marker.ARROW velocity_viz.action = Marker.ADD velocity_viz.pose.position = translation velocity_viz.pose.orientation = Quaternion(*(q.tolist())) velocity_viz.scale = Vector3(norm, 0.05, 0.05) velocity_viz.color = ColorRGBA(1, 0, 0, 0.5) traj_viz.markers.append(velocity_viz) i += step viz_pub.publish(traj_viz) # visualize information of the next 3 waypoints if num_points >= 3: p1 = traj.points[0] p2 = traj.points[1] p3 = traj.points[2] waypoints_info_text = OverlayText() waypoints_info_text.action = OverlayText.ADD waypoints_info_text.width = 128 waypoints_info_text.height = 64 waypoints_info_text.left = 0 waypoints_info_text.top = 0 waypoints_info_text.bg_color = ColorRGBA(0, 0, 0, 0.5) waypoints_info_text.fg_color = ColorRGBA(1, 1, 1, 1) waypoints_info_text.text_size = 8 waypoints_info_text.text = """Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f} Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f} Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f} """.format(p1.time_from_start.to_sec(), p1.transforms[0].translation.x, p1.transforms[0].translation.y, p1.transforms[0].translation.z, magnitude(p1.velocities[0].linear), p2.time_from_start.to_sec(), p2.transforms[0].translation.x, p2.transforms[0].translation.y, p2.transforms[0].translation.z, magnitude(p2.velocities[0].linear), p3.time_from_start.to_sec(), p3.transforms[0].translation.x, p3.transforms[0].translation.y, p3.transforms[0].translation.z, magnitude(p3.velocities[0].linear),) waypoints_info_pub.publish(waypoints_info_text)
def disable(self): msg = OverlayText() msg.action = OverlayText.DELETE self.pub.publish(msg)