Hello,
I’m working with the BlueROV simulation package from this repository: orca4. We have a physical ROV controlled by a Pixhawk flight controller running the ArduSub firmware. We’ve performed various tests in both simulation and real life, and the simulation has been effective. Now, I want to test a DVL in the simulation before purchasing one for real-world application.
In Gazebo, the DVL sensor is integrated by including the DVL plugin. This plugin simulates the DVL functionality by providing velocity data based on the Doppler effect.
The plugin is added to the Gazebo world file with the following configuration:
<plugin
filename="libgz-sim-dvl-system.so"
name="gz::sim::systems::DopplerVelocityLogSystem">
</plugin>
The sensor is then defined in the model.sdf file, which specifies its properties such as update rate, visualization options, and beam arrangement. The DVL sensor configuration is as follows:
<!-- Adding the DVL sensor -->
<sensor name="dvl_sensor" type="custom" gz:type="dvl">
<always_on>true</always_on>
<update_rate>30.0</update_rate>
<topic>/model/seabot/sensor/dvl</topic>
<visualize>true</visualize>
<gz:dvl>
<arrangement degrees="true">
<beam id="1">
<aperture>90</aperture>
<rotation>0</rotation>
<tilt>5</tilt>
</beam>
<beam id="2">
<aperture>90</aperture>
<rotation>90</rotation>
<tilt>5</tilt>
</beam>
<beam id="3">
<aperture>90</aperture>
<rotation>180</rotation>
<tilt>5</tilt>
</beam>
<beam id="4">
<aperture>90</aperture>
<rotation>270</rotation>
<tilt>5</tilt>
</beam>
</arrangement>
<tracking>
<bottom_mode>
<when>always</when>
<noise type="gaussian">
<stddev>0.01</stddev>
</noise>
<visualize>true</visualize>
</bottom_mode>
</tracking>
<minimum_range>0.1</minimum_range>
<maximum_range>100.0</maximum_range>
<resolution>0.01</resolution>
</gz:dvl>
</sensor>
This configuration defines a basic DVL setup that provides velocity data based on the simulated environment.
This script has been written to publish the DVL sensor data to a ROS topic named /DVL_vel
from gz.transport13 import Node
from gz.msgs10.dvl_kinematic_estimate_pb2 import DVLKinematicEstimate
from gz.msgs10.dvl_velocity_tracking_pb2 import DVLVelocityTracking
from rclpy.node import Node as RosNode
import rclpy
from geometry_msgs.msg import Vector3Stamped
import time
class DVLPublisher(RosNode):
def __init__(self):
super().__init__('twist_publisher')
self.publisher_ = self.create_publisher(Vector3Stamped, '/DVL_vel', 10)
def dvl_cb(self, gz_msg: DVLVelocityTracking):
# Creating a ROS message to publish
ros_msg = Vector3Stamped()
ros_msg.header.stamp = self.get_clock().now().to_msg()
ros_msg.header.frame_id = 'dvl_frame'
ros_msg.vector.x = gz_msg.velocity.mean.x
ros_msg.vector.y = gz_msg.velocity.mean.y
ros_msg.vector.z = gz_msg.velocity.mean.z
# Publishing the ROS message
self.publisher_.publish(ros_msg)
def main():
# create a transport node
gz_node = Node()
rclpy.init()
dvl_node=DVLPublisher()
topic_DVLVel = "/model/seabot/sensor/dvl"
if gz_node.subscribe(DVLVelocityTracking, topic_DVLVel, dvl_node.dvl_cb):
dvl_node.get_logger().info("Subscribing to type {} on topic [{}]".format(
DVLVelocityTracking, topic_DVLVel))
else:
dvl_node.get_logger().error("Error subscribing to topic [{}]".format(topic_DVLVel))
return
# Create a loop to keep the program running and handle callbacks
try:
while rclpy.ok():
rclpy.spin_once(dvl_node)
time.sleep(0.001)
except KeyboardInterrupt:
pass
finally:
dvl_node.get_logger().info("Shutting down...")
rclpy.shutdown()
if __name__ == '__main__':
main()
I’m getting reasonable velocity values along the 3 Axis (with some noise when robot is not moving but when i move the robot in simulation the velocity values seems correct)
i read BlueOS-Water-Linked-DVL integration example and what i understood is that to feed Ardusub’s EKF3 with DVL data we can use either VISION_POSITION_ESTIMATE or VISION_SPEED_ESTIMATE or VISION_POSITION_DELTA so i chose VISION_SPEED_ESTIMATE since that’s the direct data we got from DVL
next i added vision_speed to mavros plugins allowlist
plugin_allowlist:
- sys_status
- command
- imu
- local_position
- rc_io
- setpoint_position
- vision_speed
and this script sends the data to mavros topic that will convert it into VISION_SPEED_ESTIMATE message
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Vector3Stamped
class DVLSubscriber(Node):
def __init__(self):
super().__init__('dvl_subscriber')
self.subscription = self.create_subscription(
Vector3Stamped,
'/DVL_vel',
self.listener_callback,
10
)
self.mavros_publisher = self.create_publisher(Vector3Stamped, '/mavros/vision_speed/speed_vector', 10)
def listener_callback(self, msg: Vector3Stamped):
self.get_logger().info('Received DVL velocity: x={}, y={}, z={}'.format(
msg.vector.x, msg.vector.y, msg.vector.z
))
self.mavros_publisher.publish(msg)
def main(args=None):
rclpy.init(args=args)
dvl_subscriber = DVLSubscriber()
try:
rclpy.spin(dvl_subscriber)
except KeyboardInterrupt:
pass
finally:
dvl_subscriber.get_logger().info("Shutting down...")
dvl_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
the VISION_SPEED_ESTIMATE message is well received in QGC
the problem is that ArduSub is not sending LOCAL_POSITION_NED message (it’s not in the list of received messages from pixhawk)
those Are ArduSub params set:
EK2_ENABLE 0
EK3_ENABLE 1
AHRS_EKF_TYPE 3
GPS_TYPE 0 # Disable GPS
VISO_TYPE 1 # External vision
EK3_SRC1_POSXY 6 # External nav
EK3_SRC1_VELXY 6 # External nav
EK3_SRC1_POSZ 1 # Baro is the primary z source
EK3_SRC1_VELZ 6 # External nav z velocity influences EKF
PSC_ACCZ_P 2
PSC_ACCZ_I 4
PSC_ACCZ_FF 0.75
PSC_VELZ_P 8
PSC_POSZ_P 3
PSC_POSXY_P 2.5
PSC_VELXY_D 0.8
PSC_VELXY_I 0.5
PSC_VELXY_P 5.0
Is my understanding about how to use DVL data with Ardusub correct or not? (i mean does really Ardusub use VISION_SPEED_ESTIMATE with EKF or it’s only VISION_POSITION_ESTIMATE and VISTION_POSITION_DELTA ?)
Also do you have suggestion how can i fix the current code or how to use the DVL velocity in Ardusub to help measuring local position
Any help would be appreciated
Thank you