16 from __future__
import print_function
20 from datetime
import datetime
21 from std_msgs.msg
import Bool
22 from nav_msgs.msg
import Path
23 from visualization_msgs.msg
import MarkerArray, Marker
24 from geometry_msgs.msg
import PoseStamped, Point, Quaternion
25 from uuv_control_msgs.msg
import Trajectory, TrajectoryPoint, WaypointSet
26 import uuv_trajectory_generator
61 if rospy.has_param(
'~output_dir'):
64 print(
'Invalid output directory, not saving the files, dir=', self.
_output_dir)
73 'trajectory_marker', Path, queue_size=1)
76 'waypoint_markers', MarkerArray, queue_size=1)
79 'waypoint_path_marker', Path, queue_size=1)
82 'reference_marker', Marker, queue_size=1)
89 waypoint_path_marker = Path()
91 waypoint_path_marker.header.stamp = t
92 waypoint_path_marker.header.frame_id =
'world' 94 waypoint_marker = MarkerArray()
96 marker.header.stamp = t
97 marker.header.frame_id =
'world' 99 marker.type = Marker.SPHERE
102 waypoint_path_marker = self._waypoints.to_path_marker()
103 waypoint_path_marker.header.frame_id = self._waypoints.inertial_frame_id
104 waypoint_marker = self._waypoints.to_marker_list()
106 self._waypoint_path_pub.publish(waypoint_path_marker)
107 self._waypoint_markers_pub.publish(waypoint_marker)
110 traj_marker.header.stamp = rospy.Time.now()
111 traj_marker.header.frame_id =
'world' 114 traj_marker.header.frame_id = self._trajectory.header.frame_id
115 for pnt
in self._trajectory.points:
116 p_msg = PoseStamped()
117 p_msg.header.stamp = pnt.header.stamp
118 p_msg.header.frame_id = self._trajectory.header.frame_id
119 p_msg.pose = pnt.pose
120 traj_marker.poses.append(p_msg)
122 self._trajectory_path_pub.publish(traj_marker)
130 self._waypoints.from_message(msg)
143 marker.header.stamp = rospy.Time.now()
144 marker.header.frame_id = msg.header.frame_id
146 marker.type = Marker.SPHERE
147 marker.action = Marker.MODIFY
149 marker.pose.position = msg.pose.position
159 self._reference_marker_pub.publish(marker)
161 if __name__ ==
'__main__':
162 print(
'Starting trajectory and waypoint marker publisher')
163 rospy.init_node(
'trajectory_marker_publisher')
168 except rospy.ROSInterruptException:
169 print(
'caught exception')
_station_keeping_mode_sub
def _update_traj_tracking_mode(self, msg)
def _update_markers(self, event)
def _update_station_keeping_mode(self, msg)
def _update_waypoints(self, msg)
def _reference_callback(self, msg)
def _update_trajectory(self, msg)
def _update_auto_mode(self, msg)