def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
(secs, nsecs) = float_sec_to_int_sec_nano(self.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs).to_msg()
p_msg.pose.position = geometry_msgs.Point(x=self.p[0],
y=self.p[1],
z=self.p[2])
p_msg.pose.orientation = geometry_msgs.Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
p_msg.velocity.linear = geometry_msgs.Vector3(x=self.v[0],
y=self.v[1],
z=self.v[2])
p_msg.velocity.angular = geometry_msgs.Vector3(x=self.w[0],
y=self.w[1],
z=self.w[2])
p_msg.acceleration.linear = geometry_msgs.Vector3(x=self.a[0],
y=self.a[1],
z=self.a[2])
p_msg.acceleration.angular = geometry_msgs.Vector3(x=self.alpha[0],
y=self.alpha[1],
z=self.alpha[2])
return p_msg
def to_message(self):
"""Convert current data to a trajectory point message."""
p_msg = TrajectoryPointMsg()
p_msg.header.stamp = rospy.Time(self.t)
p_msg.pose.position = geometry_msgs.Vector3(*self.p)
p_msg.pose.orientation = geometry_msgs.Quaternion(*self.q)
p_msg.velocity.linear = geometry_msgs.Vector3(*self.v)
p_msg.velocity.angular = geometry_msgs.Vector3(*self.w)
p_msg.acceleration.linear = geometry_msgs.Vector3(*self.a)
p_msg.acceleration.angular = geometry_msgs.Vector3(*self.alpha)
return p_msg
def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
p_msg.header.stamp = rospy.Time(self.t)
p_msg.pose.position = geometry_msgs.Vector3(*self.p)
p_msg.pose.orientation = geometry_msgs.Quaternion(*self.q)
p_msg.velocity.linear = geometry_msgs.Vector3(*self.v)
p_msg.velocity.angular = geometry_msgs.Vector3(*self.w)
p_msg.acceleration.linear = geometry_msgs.Vector3(*self.a)
p_msg.acceleration.angular = geometry_msgs.Vector3(*self.alpha)
return p_msg