def gen_trajectory_message(self):
if self._points is None:
return None
msg = uuv_control_msgs.Trajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
if not self.is_using_waypoints:
for p in self._points:
p_msg = uuv_control_msgs.TrajectoryPoint()
p_msg.header.stamp = rospy.Time(p.t)
p_msg.pose.position = Vector3(*p.p)
p_msg.pose.orientation = Quaternion(*p.q)
p_msg.velocity.linear = Vector3(*p.v)
p_msg.velocity.angular = Vector3(*p.w)
p_msg.acceleration.linear = Vector3(*p.a)
p_msg.acceleration.angular = Vector3(*p.alpha)
msg.point.append(p_msg)
else:
dt = 0.05 * self._wp_interp.get_max_time()
for ti in np.arange(0, self._wp_interp.get_max_time(), dt):
pnt = self._wp_interp.interpolate(ti)
p_msg = uuv_control_msgs.TrajectoryPoint()
p_msg.header.stamp = rospy.Time(pnt.t)
p_msg.pose.position = Vector3(*pnt.p)
p_msg.pose.orientation = Quaternion(*pnt.q)
p_msg.velocity.linear = Vector3(*pnt.v)
p_msg.velocity.angular = Vector3(*pnt.w)
p_msg.acceleration.linear = Vector3(*pnt.a)
p_msg.acceleration.angular = Vector3(*pnt.alpha)
msg.point.append(p_msg)
return msg
def get_trajectory_as_message(self):
"""
Return the trajectory points as a Trajectory type message. If waypoints
are currently in use, then sample the interpolated path and return the
poses only.
"""
if self.points is None:
return None
msg = uuv_control_msgs.Trajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
for pnt in self.points:
msg.points.append(pnt.to_message())
return msg
def get_trajectory_as_message(self):
"""
Return the trajectory points as a Trajectory type message. If waypoints
are currently in use, then sample the interpolated path and return the
poses only.
"""
if self.points is None:
return None
msg = uuv_control_msgs.Trajectory()
try:
msg.header.stamp = self.node.get_clock().now().to_msg()
except:
self._logger.warning('A ROS node was not initialized, no '
'timestamp can be set to Trajectory message')
msg.header.frame_id = 'world'
for pnt in self.points:
# FIXME Sometimes the time stamp of the point is NaN
msg.points.append(pnt.to_message())
return msg
def get_trajectory_as_message(self):
"""
Return the trajectory points as a Trajectory type message. If waypoints
are currently in use, then sample the interpolated path and return the
poses only.
"""
try:
if self.points is None:
return None
msg = uuv_control_msgs.Trajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'world'
for pnt in self.points:
# FIXME Sometimes the time stamp of the point is NaN
msg.points.append(pnt.to_message())
return msg
except:
print 'Error during creation of trajectory message'
return None
def gen_trajectory_message(self):
if self._points is None:
return None
msg = uuv_control_msgs.Trajectory()
try:
msg.header.stamp = rospy.Time.now()
set_timestamps = True
except:
set_timestamps = False
self._logger.warning('ROS node was not initialized, no timestamp '
'can be assigned to trajectory message')
msg.header.frame_id = 'world'
if not self.is_using_waypoints:
for p in self._points:
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
p_msg.header.stamp = rospy.Time(p.t)
p_msg.pose.position = Vector3(*p.p)
p_msg.pose.orientation = Quaternion(*p.q)
p_msg.velocity.linear = Vector3(*p.v)
p_msg.velocity.angular = Vector3(*p.w)
p_msg.acceleration.linear = Vector3(*p.a)
p_msg.acceleration.angular = Vector3(*p.alpha)
msg.points.append(p_msg)
else:
dt = 0.05 * self._wp_interp.get_max_time()
for ti in np.arange(0, self._wp_interp.get_max_time(), dt):
pnt = self._wp_interp.interpolate(ti)
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
p_msg.header.stamp = rospy.Time(pnt.t)
p_msg.pose.position = Vector3(*pnt.p)
p_msg.pose.orientation = Quaternion(*pnt.q)
p_msg.velocity.linear = Vector3(*pnt.v)
p_msg.velocity.angular = Vector3(*pnt.w)
p_msg.acceleration.linear = Vector3(*pnt.a)
p_msg.acceleration.angular = Vector3(*pnt.alpha)
msg.points.append(p_msg)
return msg
def gen_trajectory_message(self):
if self._points is None:
return None
msg = uuv_control_msgs.Trajectory()
try:
msg.header.stamp = self.node.get_clock().now().to_msg()
set_timestamps = True
except:
set_timestamps = False
self._logger.warning('ROS node was not initialized, no timestamp '
'can be assigned to trajectory message')
msg.header.frame_id = 'world'
#TODO Simplify following
if not self.is_using_waypoints:
for p in self._points:
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(p.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
p_msg.pose.position = Vector3(x=p.p[0], y=p.p[1], z=p.p[2])
p_msg.pose.orientation = Quaternion(x=p.q[0],
y=p.q[1],
z=p.q[2],
w=p.q[3])
p_msg.velocity.linear = Vector3(x=p.v[0], y=p.v[1], z=p.v[2])
p_msg.velocity.angular = Vector3(x=p.w[0], y=p.w[1], z=p.w[2])
p_msg.acceleration.linear = Vector3(x=p.a[0],
y=p.a[1],
z=p.a[2])
p_msg.acceleration.angular = Vector3(x=p.alpha[0],
y=p.alpha[1],
z=p.alpha[2])
msg.points.append(p_msg)
else:
dt = 0.05 * self._wp_interp.get_max_time()
for ti in np.arange(0, self._wp_interp.get_max_time(), dt):
pnt = self._wp_interp.interpolate(ti)
p_msg = uuv_control_msgs.TrajectoryPoint()
if set_timestamps:
(secs, nsecs) = float_sec_to_int_sec_nano(pnt.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs)
p_msg.pose.position = Vector3(x=pnt.p[0],
y=pnt.p[1],
z=pnt.p[2])
p_msg.pose.orientation = Quaternion(x=pnt.q[0],
y=pnt.q[1],
z=pnt.q[2],
w=pnt.q[3])
p_msg.velocity.linear = Vector3(x=pnt.v[0],
y=pnt.v[1],
z=pnt.v[2])
p_msg.velocity.angular = Vector3(x=pnt.w[0],
y=pnt.w[1],
z=pnt.w[2])
p_msg.acceleration.linear = Vector3(x=pnt.a[0],
y=pnt.a[1],
z=pnt.a[2])
p_msg.acceleration.angular = Vector3(x=pnt.alpha[0],
y=pnt.alpha[1],
z=pnt.alpha[2])
msg.points.append(p_msg)
return msg
