ROS support for BlueROV2

@olaya,

If you want to try, we have a WIP ROS package, it includes a gazebo simulation with/without ROS integration and a bridge to control the ROV via ROS topics.

That looks great, thank you!

I’ll try to use it, and I will give you any feedback in the usage! :smile:

@olaya, did you already test it?

@patrickelectric I’am testing your package, but i have a problem, i cannot override in every channel just in 3 and 5. Did you know what is the problem?

Are you sending rc values like this ?
rostopic pub -r 10 /BlueRov2/rc_channel2/set_pwm std_msgs/UInt16 1100
Can you say exactly what are you performing and how ?

Yes, it’s strange. Now when i send PWM to RC 5 channel, i have two motors spinning. :smile:

I have the configuration with Pixhawk, RPI3 with the original image in ArduSub website, connected by Ethernet to a Top side PC (like as in above image). In the top side PC i’am running the just the ROS core and the node of BlueRov2. In this moment i’am just testing and try to control motors individual.

I see now what are you trying to do :thinking:
The rc_channel topic follows rc-input-and-output documentation.

Take a look in the BlueRov2 frame:
image

RC Inputs
These are the default channel mappings for RC input:
Channel	Meaning
1	    Pitch
2       Roll
3	    Throttle
4	    Yaw
5	    Forward
6	    Lateral
7	    Reserved
8	    Camera Tilt
9	    Lights 1 Level
10	    Lights 2 Level

Each /BlueRov2/rc_channelX/set_pwm, where X is the channel number, send a RC value like the joystick.

  • /BlueRov2/rc_channel1: Will not work because the frame doesn’t have pitch control.
  • /BlueRov2/rc_channel2: Will spin 6 and 5
  • /BlueRov2/rc_channel3: Will spin 6 and 5
  • /BlueRov2/rc_channel4: Will spin 1,2,3 and 4 to perform yaw movement
  • /BlueRov2/rc_channel5: Will spin 1,2,3 and 4 to perform forward movement
  • /BlueRov2/rc_channel6: Will spin 1,2,3 and 4 to perform lateral movement
  • /BlueRov2/rc_channel8: Will move the camera servo
  • /BlueRov2/rc_channel9-10: Will change the light state

How the motors spin will depend of the BlueRov mode (guided, stabilize, etc…)

1 Like

Oh, now i understand! Grateful :smiley: It will help me for my propose. My misunderstanding was when i “override” to a RC channel i was thinking that this action is a Digital Pin Write function (Arduino). But, if i understand, ArduSub works in some high level abstraction (in a concept of radio modelism), channels controls the “movement” not a digital pin, right?

But, with curiosity, I have already try with mavros without success too:

roslaunch bluerov_ros_playground user_mav.launch
rostopic pub /mavros/rc/override mavros_msgs/OverrideRCIn "channels: [1600, 1500, 1500, 1500, 1500, 1500, 1500, 1500]" -r 10

Why not work? :thinking:

Yes, exactly, take a look in ArduSub documentation about RC params list.

Like I said before:
/BlueRov2/rc_channel1: Will not work because the frame doesn’t have pitch control.

Probably It’ll do something if you ARM the vehicle and use something like channels: [1500, 1600, 1500, 1500, 1500, 1500, 1500, 1500]

Ok, i will!

Sorry, i’m :sleeping: Thanks a lot for your help :smiley:

Hello Patrick,
Regarding this channel configuration, I’ve consulted this link where it says that:

there are 8 main outputs and 6 auxiliary outputs. It is important not to connect analog servos to the main outputs, because the very fast update rate can cause the servo to burn itself out.

However, it is not clear to me which are the “main” and which the “auxiliar”.
I added an additional thruster to my bluerov2, and I want to run it when pressing the directional pad up and down. I decided to connect it to the light level channels, and modify the ROS node user_mav in order to run it.

However, I found that mavros/rc/override only publishes in 8 channels. How is the correspondence with the channels you mentioned above? Is there any other way that the light level channels should be accessed?

Thank you!:smiley:

Hi Olaya,

This is related to the pixhawk servo array.
image
AUX OUT: Auxiliar output, MAIN OUT: Main output.

In the exactly same way as in my last post :slight_smile:

Yes, that’s correct. But you can connect a ESC in the auxiliary port, the frequency will not be so high, but it’s not a problem for the ESC.

Oh sorry, I didn’t realize that rc_channel9-10 referred to the 8th field of the mavros override topic!

About the values, do all of them need to be between 1100 and 1900?

I have the camera servo connected to MAIN8 in the pixhawk, and it presents a very strange behavior, very different to when it is commanded through the QGroundControl.

On the other hand, the lights should be in MAIN7, so I connected the additional thruster there, but nothing happens! maybe I have the connections wrong? I did them according to this page.

Additionally, is there a way to send commands to the aux ports through ROS? I didn’t find any documentation on this!

Thank you very much for your help :blush:

Hi Olaya,

Sorry for taking this long to reply. I’ll need to update my ROS system here to do further test and help you with that. It can take some time.

Rc_override is to write over the joystick signals and not the pixhawk output, you can check my previous post.

Are you using mavros or bluerov bridge ?

That’s to maintain compatibility with legacy code, a result of RC radios.

Directly no, you’ll need to modify ArduSub to use rc_override to bypass the values for the aux ports.

Slightly off topic but, Patrick, your ROS package has topics like /BlueRov2/servo1/set_pwm and etc. Are these topics used to set the PWM for the thrusters directly? I have tried writing to them but nothing happens so I am just not sure what they are for. Is the a way to PWM the thrusters directly with your package? That is what I am primarily interested in. Thanks for your time.

@brandonk Those topics will only work for outputs that are not configured as motors. There is no way to set pwm to thrusters directly via ROS. You may only set the desired roll/pitch/yaw and forward/up/down.

You can check and see when we will fix this here: https://github.com/bluerobotics/ardusub/issues/166

There is a hack you could do and use the MOTOR_TEST command via mavlink. The command is meant to be used for motor testing, but the logic is almost the same. This will require a pretty high message frequency though, so try higher baudrates.

Hi @brandonk,

Yes, Jacob is correct, the ROS layer only provides access to the mavlink commands, we are always improving ArduSub to allow a more more inputs and controls :slight_smile:

Hi again Patrick!

I want to read the data from the Bar30 pressure sensor with your ROS node.
I can see there are two topics that are being published:

  • /mavros/imu/diff_pressure
  • /mavros/imu/static_pressure

Does any of this topics publish the bar30 info? Or is it the data from the pixhawk barometer?
Moreover, the name diff_pressure confuses me. Is it a differential value? And in case it is, a difference between what?

Thank you!!! :slight_smile:

Hi,

mavros uses SCALED_PRESSURE (that’s the internal pixhawk barometer) and not SCALED_PRESSURE2 (that contains bar30 data).
To get the bar30 value, you’ll need to create a subscriber to mavlink/from (message type: mavros_msgs/Mavlink) topic, that streams from autopilot the mavlink messages.
We have some pymavlink examples here, you can use it to convert the raw data from the mavros topic to python mavlink structures.

Hello Patrick, thank you for your answer!

I saw that the topic /mavlink/from returns different arrays of values with different IDs. I guess that the IDs in the ROS topics correspond to those of the mavlink messages.
The structure and values of the topic with the SCALED_PRESSURE2 are as follows:

header
	seq:  554
	stamp
	frame_id: 
framing_status:  1
magic:  254
len:  14
incompat_flags:  0
compat_flags:  0
seq:  117
sysid:  1
compid:  1
msgid:  137
checksum:  38534
payload64
	[0]:  4929168678098075594
	[1]:  5078067614371020
signature: 

I guess that those two values are the absolute and differential pressure. According to sensor documentation, the bar30 is supposed to provide its readings in mbar; however the values are extremely high! What am I missing?

Thank you again!

Hi,

Sorry for the delay, my virtual machine running ROS was not working properly.

The payload64 list is a bytearray that need to be converted to python structure.
This message has 16 bytes, and the SCALED_PRESSURE2 only uses 14 (uint32_t, float, float, int16_t), so it’s necessary to create a pack/unpack method in python to decode the necessary info:

#!/usr/bin/env python

# ROS module
import rospy
# Mavlink ROS messages
from mavros_msgs.msg import Mavlink
# pack and unpack functions to deal with the bytearray
from struct import pack, unpack

# Topic callback
def callback(data):
    # Check if message id is valid (I'm using SCALED_PRESSURE
    # and not SCALED_PRESSURE2)
    if data.msgid == 29:
        rospy.loginfo(rospy.get_caller_id() + " Package: %s", data)
        # Transform the payload in a python string
        p = pack("QQ", *data.payload64)
        # Transform the string in valid values
        # https://docs.python.org/2/library/struct.html
        time_boot_ms, press_abs, press_diff, temperature = unpack("Iffhxx", p)
    
def listener():
    rospy.init_node('listener', anonymous=True)

    rospy.Subscriber("/mavlink/from", Mavlink, callback)

    rospy.spin()

if __name__ == '__main__':
    listener()