def publish_auv_command(self, surge_speed, wrench):
"""Publish the AUV control command message
> *Input arguments*
* `surge_speed` (*type:* `float`): Reference surge speed
* `wrench` (*type:* `numpy.array`): 6 DoF wrench vector
"""
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace,
self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = max(self._min_thrust, wrench[0])
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)
def publish_auv_command(self, surge_speed, wrench):
if not self.odom_is_init:
return
surge_speed = max(0, surge_speed)
msg = AUVCommand()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '%s/%s' % (self._namespace, self._vehicle_model.body_frame_id)
msg.surge_speed = surge_speed
msg.command.force.x = wrench[0]
msg.command.force.y = wrench[1]
msg.command.force.z = wrench[2]
msg.command.torque.x = wrench[3]
msg.command.torque.y = wrench[4]
msg.command.torque.z = wrench[5]
self._auv_command_pub.publish(msg)