Multi-vehicle Gimbal Control Failed

[andy zhuo]

Hello,
I'm trying to control gimbals for different uavs(typhoon_h480_0, typhoon_1 ... etc.) at the same time. There is something wrong when I use topic /typhoon_h480_0/mavros/mount_control/command to control the gimbal of typhoon_h480_0. In Gazebo simulator, the moving gimbal is uav_1(in later attempts, it also occurs to uav2), instead of desired uav_0. The test cammand change the pitch to -60 degree.

And rosgraph shows the node and topic.

PX4 current version is 1.13.2, Gazebo version is 11 and ROS is melodic version.
The launch file I used is based on origin launch multi_uav_siti_sdf.launch and I just change the vehicle name from plane to typhoon_h480, along with group ns.

is there something I missed? Any suggestion would help, thx.

By the way, I have searched issues and found that issue #540 (2 years ago) situation is quite similar to me. However, the bug seems to have been fixed.

[Jaeyoung Lim]

@andy-zhuo-02 I don't think this is really a bug.

The launchfile in https://github.com/Jaeyoung-Lim/mavros_humantracking might be helpful for you

[Peixuan Shu]

@andy-zhuo-02 The gazebo_gimbal_controller_plugin in PX4 v1.13 used a fixed udp port dest_addr.sin_port = htons(13030), which is not applicable to multiple gimbals situation:

PX4-SITL_gazebo-classic/src/gazebo_gimbal_controller_plugin.cpp

Lines 743 to 757 in 48440d7

 void GimbalControllerPlugin::SendMavlinkMessage(const mavlink_message_t &msg) 
 { 
   uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; 
   const int packetlen = mavlink_msg_to_send_buffer(buffer, &msg); 
  
   sockaddr_in dest_addr {}; 
   dest_addr.sin_family = AF_INET; 
   inet_pton(AF_INET, "127.0.0.1", &dest_addr.sin_addr.s_addr); 
   dest_addr.sin_port = htons(13030); 
  
   const ssize_t len = sendto(this->sock, buffer, packetlen, 0, reinterpret_cast<sockaddr *>(&dest_addr), sizeof(dest_addr)); 
   if (len <= 0) { 
     gzerr << "Failed sending mavlink message: " << strerror(errno) << endl; 
   } 
 } 

This bug has been fixed in PX4 v1.14 or higher version, which adds a sdf element 'udp_gimbal_port_remote' to determine the gimbal control port:

PX4-SITL_gazebo-classic/src/gazebo_gimbal_controller_plugin.cpp

Lines 361 to 367 in 1c5818e

   if (this->sdf->HasElement("udp_gimbal_port_remote")) { 
     this->udp_gimbal_port_remote = _sdf->Get<int>("udp_gimbal_port_remote"); 
   } else { 
     this->udp_gimbal_port_remote = 13030; 
   } 
   gzwarn << "[gazebo_gimbal_controller_plugin] Streaming gimbal mavlink stream to ip: " << this->udp_gimbal_host_ip  << " port: " << this->udp_gimbal_port_remote << std::endl; 
 } 

PX4-SITL_gazebo-classic/src/gazebo_gimbal_controller_plugin.cpp

Line 768 in 1c5818e

 dest_addr.sin_port = htons(this->udp_gimbal_port_remote); 

Therefore, use the gazebo_gimbal_controller_plugin of PX4 v1.14 as listed above, and add a udp_gimbal_port_remote element to the gimbal plugin in typhoon_h480 sdf model:

PX4-SITL_gazebo-classic/models/typhoon_h480/typhoon_h480.sdf.jinja

Lines 1433 to 1439 in 1c5818e

 <plugin name='gimbal_controller' filename='libgazebo_gimbal_controller_plugin.so'> 
   <joint_yaw>typhoon_h480::cgo3_vertical_arm_joint</joint_yaw> 
   <joint_roll>typhoon_h480::cgo3_horizontal_arm_joint</joint_roll> 
   <joint_pitch>typhoon_h480::cgo3_camera_joint</joint_pitch> 
   <control_gimbal_channels> 
     <channel> 
       <joint_control_pid> 

, which should be like:

 <plugin name='gimbal_controller' filename='libgazebo_gimbal_controller_plugin.so'> 
   <udp_gimbal_port_remote> 13030 </udp_gimbal_port_remote>
   <joint_yaw>typhoon_h480::cgo3_vertical_arm_joint</joint_yaw> 
   <joint_roll>typhoon_h480::cgo3_horizontal_arm_joint</joint_roll> 
   <joint_pitch>typhoon_h480::cgo3_camera_joint</joint_pitch> 

Change the udp_gimbal_port_remote for multiple typhoon models as 13030, 13031, etc., which means you should create multiple typhoon_h480 sdf model and launch them seperately.

Then, you can control multiple gimbals simultaneously.

[andy zhuo]

Your method solved my problem perfectly. Thank you.