patrickelectric / bluerov_ros_playground

Scripts to help BlueRov integration with ROS
68 stars 76 forks source link

Empty topics with bluerov_node #30

Open olayasturias opened 6 years ago

olayasturias commented 6 years ago

When launching the bluerov2 node by doing: roslaunch bluerov_ros_playground bluerov2_node.launch(there's a mistake in the README, where bluerov_node is called)

The following text appears in the console:

no BATTERY_STATUS data
no HEARTBEAT data
no SCALED_IMUX data
no LOCAL_POSITION_NED data
no SCALED_IMUX data
no LOCAL_POSITION_NED data

After that, I echoed the topics /BlueRov2/battery, /BlueRov2/imu/data, and odometry, and nothing happens. Maybe they aren't supposed to publish anything? However, the image and state topics are publishing the topics well.

On the other hand, when launching roslaunch bluerov_ros_playground user_mav.launch the imu data is correctly displayed in the topic /mavros/imu/data (with a very high error in the Z axis, though).

nathanfourniol commented 5 years ago

Hello ! I had the same issue. I solved it by adding two more lines in the file bridge.py :

class Bridge(object):                                                                                                                                                                                             
      """ MAVLink bridge                                                                                                                                                                                            

      Attributes:                                                                                                                                                                                                   
      ┊   conn (TYPE): MAVLink connection                                                                                                                                                                           
      ┊   data (dict): Deal with all data                                                                                                                                                                           
      """                                                                                                                                                                                                           
      def __init__(self, device='udpin:192.168.2.1:14550', baudrate=115200):                                                                                                                                        
      ┊   """                                                                                                                                                                                                       
      ┊   Args:                                                                                                                                                                                                     
      ┊   ┊   device (str, optional): Input device                                                                                                                                                                  
      ┊   ┊   ┊   https://ardupilot.github.io/MAVProxy/html/getting_started/starting.html#master                                                                                                                    
      ┊   ┊   baudrate (int, optional): Baudrate for serial communication                                                                                                                                           
      ┊   """                                                                                                                                                                                                       
      ┊   self.conn = mavutil.mavlink_connection(device, baud=baudrate)

       #--------Lines to initiate mavlink data stream-----------                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             
      ┊   self.conn.wait_heartbeat()                                                                                                                                                                              
      ┊   self.conn.mav.request_data_stream_send(self.conn.target_system, self.conn.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL,4, 1)                                                                                                                                                                                                                                                                                                                   
       #---------------------------
      ┊   self.data = {}       
       .....           

If you want more information about the parameters of the request_data_stream_send method you can check https://mavlink.io/en/mavgen_python/#listen to see how to use pymavlink and https://mavlink.io/en/messages/common.html for the parameters (e.g. MAV_DATA_STREAM_ALL)

Otherwise, if you start QGroundControl at the same time or before launching the bluerov2 node, it works for me.

BR, Nathan