patrickelectric / bluerov_ros_playground

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

Running "bluerov node.py" cannot obtain bluerov2 data information #38

Open Hondlus opened 5 years ago

Hondlus commented 5 years ago

show:no SYS_STATUS data no SERVO_OUTPUT_RAW data no ATTITUDE data no LOCAL_POSITION_NED data

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