search

PX4 / PX4-SITL_gazebo-classic

Set of plugins, models and worlds to use with OSRF Gazebo Simulator in SITL and HITL.
http://dev.px4.io/simulation-gazebo.html
375 stars 787 forks source link

Controlling gimbals for multiple vehicles using ROS node in SITL #540

Closed Jaroan closed 4 years ago

Jaroan commented 4 years ago

I want to control the gimbal of two Typhoon H480 models in an offboard SITL setup using the topic /mavros/mount_control/command. I have understood how to implement it for a single model with help from this repo Jaeyoung-Lim/mavros_humantracking. My implementation : Gif Link For multi-UAV simulation, I tried to extend using the appropriate launch file and corresponding topics but I was not able to control the gimbals. They seemed to be unsteady as well. Video I am unable to move even one vehicle’s gimbal for a multi UAV simulation, like in the case of the single-vehicle. Any help and suggestion will be appreciated.

I tried to implement the changes given in the pull request - https://github.com/PX4/sitl_gazebo/pull/469 but I didn’t find a difference in the jitter. Is there something I’m skipping? I've also raised this issue in a Forum topic https://discuss.px4.io/t/controlling-gimbal-for-multiple-vehicles-using-ros-node-in-sitl/17415?u=jaroan :slightly_frowning_face:

Jaeyoung-Lim commented 4 years ago

@Jaroan The gimbal mavlink messages are being sent to the same udp port. You need to asign different ports for each vehicle as done with the xacro macros

Jaroan commented 4 years ago

I have the following in the Typhoon SDF File. Where do I set it for multiple vehicles? I apologise for the silly doubt

   <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
      <robotNamespace></robotNamespace>
      <imuSubTopic>/imu</imuSubTopic>
      <gpsSubTopic>/gps</gpsSubTopic>
      <magSubTopic>/mag</magSubTopic>
      <baroSubTopic>/baro</baroSubTopic>
      <mavlink_addr>INADDR_ANY</mavlink_addr>
      <mavlink_udp_port>14560</mavlink_udp_port>
      <serialEnabled>false</serialEnabled>
      <serialDevice>/dev/ttyACM0</serialDevice>
      <baudRate>921600</baudRate>
      <qgc_addr>INADDR_ANY</qgc_addr>
      <qgc_udp_port>14550</qgc_udp_port>
      <sdk_addr>INADDR_ANY</sdk_addr>
      <sdk_udp_port>14540</sdk_udp_port>
      <hil_mode>false</hil_mode>
      <hil_state_level>false</hil_state_level>
      <enable_lockstep>false</enable_lockstep>
      <use_tcp>true</use_tcp>
      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
      <control_channels>
        <channel name="rotor0">
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>

        </channel>
        <channel name="rotor1">
          <input_index>1</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name="rotor2">
          <input_index>2</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name="rotor3">
          <input_index>3</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name="rotor4">
          <input_index>4</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
       <channel name="rotor5">
          <input_index>5</input_index>
          <input_offset>0</input_offset>
          <input_scaling>1500</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>100</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
        </channel>
        <channel name="gimbal_roll">
          <input_index>6</input_index>
          <input_offset>0</input_offset>
          <input_scaling>-3.1415</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>position_gztopic</joint_control_type>
          <gztopic>/gimbal_roll_cmd</gztopic>
          <joint_name>typhoon_h480::cgo3_camera_joint</joint_name>
        </channel>
        <channel name="gimbal_pitch">
          <input_index>7</input_index>
          <input_offset>0</input_offset>
          <input_scaling>-3.1415</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>position_gztopic</joint_control_type>
          <gztopic>/gimbal_pitch_cmd</gztopic>
          <joint_name>typhoon_h480::cgo3_camera_joint</joint_name>
        </channel>
        <channel name="gimbal_yaw">
          <input_index>8</input_index>
          <input_offset>0</input_offset>
          <input_scaling>-3.1415</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>position_gztopic</joint_control_type>
          <gztopic>/gimbal_yaw_cmd</gztopic>
          <joint_name>typhoon_h480::cgo3_vertical_arm_joint</joint_name>
        </channel>
        <channel name="left_leg">
          <input_index>9</input_index>
          <input_offset>1</input_offset>
          <input_scaling>0.5</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>position</joint_control_type>
          <joint_control_pid>
            <p>3.5</p>
            <i>0.5</i>
            <d>0</d>
            <iMax>4</iMax>
            <iMin>-4</iMin>
            <cmdMax>6</cmdMax>
            <cmdMin>-6</cmdMin>
          </joint_control_pid>
          <joint_name>left_leg_joint</joint_name>
        </channel>
        <channel name="right_leg">
          <input_index>10</input_index>
          <input_offset>1</input_offset>
          <input_scaling>0.5</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>position</joint_control_type>
          <joint_control_pid>
            <p>3.5</p>
            <i>0.5</i>
            <d>0</d>
            <iMax>4</iMax>
            <iMin>-4</iMin>
            <cmdMax>6</cmdMax>
            <cmdMin>-6</cmdMin>
          </joint_control_pid>
          <joint_name>right_leg_joint</joint_name>
        </channel>
      </control_channels>
    </plugin>
Jaeyoung-Lim commented 4 years ago

@Jaroan https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post#L10 This is where the mount orientation mavlink is being streamed.

How are you launching multiple instances of typhoon_h480? I see in https://github.com/PX4/Firmware/blob/master/launch/single_vehicle_spawn.launch#L23 that you need an xacro description of the vehicle but typhoon h480 is not available as a xacro

Jaroan commented 4 years ago

Oh, I see. I am using _multi_uav_mavros_sitlsdf.launch file with the command roslaunch px4 multi_uav_mavros_sitl_sdf.launch vehicle:=typhoon_h480 to spawn two typhoon_h480s

Jaeyoung-Lim commented 4 years ago

And do they have different IDs? In this case it should have different ports assigned. You might see it in the shell which one is used

Jaroan commented 4 years ago

On my shell I have 

 * /uav0/mavros/fcu_url: udp://:14540@loca...
 * /uav1/mavros/fcu_url: udp://:14541@loca...

INFO  [simulator] Waiting for simulator to connect on TCP port 4560
process[uav1/typhoon_h480_1_spawn-8]: started with pid [15265]
process[uav1/mavros-9]: started with pid [15273]
INFO  [param] selected parameter default file eeprom/parameters_6011
[param] Loaded: eeprom/parameters_6011
[ INFO] [1593948021.608540825]: FCU URL: udp://:14540@localhost:14580
[ INFO] [1593948021.612332393]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1593948021.613643636]: udp0: Remote address: 127.0.0.1:14580

INFO  [simulator] Simulator connected on TCP port 4560.
INFO  [init] Mixer: etc/mixers/hexa_x.main.mix on /dev/pwm_output0
[uav0/typhoon_h480_0_spawn-5] process has finished cleanly
log file: /home/jasmine/.ros/log/828ae218-beb1-11ea-aa32-f828195a1467/uav0-typhoon_h480_0_spawn-5*.log
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14571 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14581 remote port 14541
INFO  [init] Mixer: etc/mixers/hexa_x.main.mix on /dev/pwm_output0
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14558 remote port 14530
10.80
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14570 remote port 14550
INFO  [logger] logger started (mode=all)

INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1593948026.798240244, 0.448000000]: udp0: Remote address: 127.0.0.1:14581
[ INFO] [1593948026.798797893, 0.448000000]: FCU: [logger] file: ./log/2020-07-05/11_20_26.ulg
[ INFO] [1593948026.800702421, 0.448000000]: IMU: High resolution IMU detected!
INFO  [px4] Startup script returned successfully
INFO  [mavlink] partner IP: 127.0.0.1
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14558 remote port 14530
WARN  [mavlink] bind failed: Address already in use
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] Opened full log file: ./log/2020-07-05/11_20_27.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
[ INFO] [1593948027.038121151, 0.628000000]: udp0: Remote address: 127.0.0.1:14580

I have provided different IDs and ports in the launch file:

    <!-- UAV0 -->
    <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
            <arg name="x" value="0"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14560"/>
            <arg name="mavlink_tcp_port" value="4560"/>
            <arg name="ID" value="$(arg ID)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
    <!-- UAV1 -->
    <group ns="uav1">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="1"/>
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
            <arg name="x" value="1"/>
            <arg name="y" value="0"/>
            <arg name="z" value="0"/>
            <arg name="R" value="0"/>
            <arg name="P" value="0"/>
            <arg name="Y" value="0"/>
            <arg name="vehicle" value="$(arg vehicle)"/>
            <arg name="mavlink_udp_port" value="14561"/>
            <arg name="mavlink_tcp_port" value="4561"/>
            <arg name="ID" value="$(arg ID)"/>
        </include>
        <!-- MAVROS -->
        <include file="$(find mavros)/launch/px4.launch">
            <arg name="fcu_url" value="$(arg fcu_url)"/>
            <arg name="gcs_url" value=""/>
            <arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
            <arg name="tgt_component" value="1"/>
        </include>
    </group>
Jaeyoung-Lim commented 4 years ago

Okay, you would need to figure out exactly where it went wrong.

The UDP / TCP ports are configured here

Jaroan commented 4 years ago

@Jaeyoung-Lim On running netstat, I am able to see all the connections for TCP based on the launch are established as below. the UDP connections also are listed

$ netstat -tn | grep 4560
tcp       93     76 127.0.0.1:4560          127.0.0.1:33146         ESTABLISHED
tcp        0      0 127.0.0.1:33146         127.0.0.1:4560          ESTABLISHED
$ netstat -tn | grep 4561
tcp        0      0 127.0.0.1:33234         127.0.0.1:4561          ESTABLISHED
tcp       93      0 127.0.0.1:4561          127.0.0.1:33234         ESTABLISHED

$ netstat -au
Active Internet connections (servers and established)
Proto Recv-Q Send-Q Local Address           Foreign Address         State      
udp        0      0 localhost:domain        0.0.0.0:*                                                    
udp        0      0 0.0.0.0:ipp             0.0.0.0:*                          
udp        0      0 0.0.0.0:14530           0.0.0.0:*                          
udp        0      0 0.0.0.0:14540           0.0.0.0:*                          
udp        0      0 0.0.0.0:14541           0.0.0.0:*                          
udp        0      0 0.0.0.0:14558           0.0.0.0:*                          
udp        0      0 0.0.0.0:14570           0.0.0.0:*                          
udp        0      0 0.0.0.0:14571           0.0.0.0:*                          
udp        0      0 0.0.0.0:14580           0.0.0.0:*                          
udp        0      0 0.0.0.0:14581           0.0.0.0:*

Is there any checks to ensure the MOUNT_ORIENTATION is set to the right ports based on this: https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post#L10

Jaeyoung-Lim commented 4 years ago

Looks like the ports are correct.

If it is not the ports, you need to find out where this is coming from

Question: does this work for a single vehicle?

Jaroan commented 4 years ago

Yes it works for a single vehicle here is a video https://imgur.com/gallery/qMO0CsH TyphoonGimbal1

Here is the code snippet for single vehicle using a controller class object which instantiate mount control message as self.mount_control = MountControl()

    # Single vehicle Mount Command
    mountCnt = rospy.Publisher('/mavros/mount_control/command', MountControl, queue_size=10)

    gimbal_pitch_ = 0.0
    gimbal_yaw_ = 0.0
    gimbal_roll_ = 0.0
.
.
.
    while not rospy.is_shutdown():

    object.mount_control.header.stamp = rospy.Time.now()
    object.mount_control.header.frame_id = "map"
    object.mount_control.mode = 2
    object.mount_control.pitch = gimbal_pitch_
    object.mount_control.roll = gimbal_roll_
    if i%20 == 0:
        gimbal_yaw_ += 10
        print("yaw increased to", gimbal_yaw_)
    object.mount_control.yaw = gimbal_yaw_
    mountCnt.publish(object.mount_control)
    i+=1
        rate.sleep()

The similar code for multiple vehicles using the appropriate namespace for topics e.g. uav0/mavros... and uav1/mavros... doesn't control the gimbal.

However running rostopic echo uav0/mavros/mount_control/command shows the yaw that is published correctly but no results on the vehicle as seen on Gazebo or Rviz image topic.

Jaeyoung-Lim commented 4 years ago

@Jaroan Are you sure /mavros/mount_control/command is the right topic to publish? Each drone should have a different topic to receive this right?

Jaroan commented 4 years ago

I am able to successfully arm, and create setpoints and takeoff both vehicles. The similar code for multiple vehicles using the appropriate namespace for topics e.g. uav0/mavros... and uav1/mavros... doesn't control the gimbal. TyphoonGimbalJitter

However running rostopic echo uav0/mavros/mount_control/command shows the yaw that is published correctly but no results on the vehicle as seen on Gazebo or Rviz image topic.

    # Mount Command
    mountCnt1 = rospy.Publisher('uav1/mavros/mount_control/command', MountControl, queue_size=10)

    mountCnt0 = rospy.Publisher('uav0/mavros/mount_control/command', MountControl, queue_size=10)

    gimbal_pitch_ = 0.0
    gimbal_yaw_ = 0.0
    gimbal_roll_ = 0.0

    while not rospy.is_shutdown():

    obj1.mount_control.header.stamp = rospy.Time.now()
    obj1.mount_control.header.frame_id = "map"
    obj1.mount_control.mode = 2
    obj1.mount_control.pitch = gimbal_pitch_
    obj1.mount_control.roll = gimbal_roll_
    obj1.mount_control.yaw = gimbal_yaw_
    if i%20 == 0:
        gimbal_yaw_ += 10
        print("Publishing yaw", obj1.mount_control.yaw)

    mountCnt1.publish(obj1.mount_control)
        i+=1
Jaeyoung-Lim commented 4 years ago

Can you check if the mount control messages are received on the firmware side?

Jaeyoung-Lim commented 4 years ago

@Jaroan Now I see the problem

You need separate sdf files for each typhoon_h480 which matches the tcp and udp ports

Jaroan commented 4 years ago

Oh ok, I have made a copy of the typhoon_h480 sdf file and modified lines to the appropriate udp port https://github.com/PX4/sitl_gazebo/blob/1af7e29dbb1ecce7b0b191c9deb24ab1f13916ab/models/typhoon_h480/typhoon_h480.sdf#L1283 as well as <qgc_udp_port>14551</qgc_udp_port> and <sdk_udp_port>14541</sdk_udp_port>

The single_vehicle_spawn_sdf.launch launch file was also duplicated appropriately and renamed to include the new sdf file and changes made to the multi_uav_mavros_sitl_sdf.launch file.

Do I need a separate startup file in Firmware/ROMFS/px4fmu_common/init.d-posix/ directory as well?

Jaeyoung-Lim commented 4 years ago

@Jaroan No, you just need two different sdf files and not duplicate the launch file. No, the statupfile that is being used should not be duplicated. Please look at other examples for multivehicle spawning as written in https://dev.px4.io/master/en/simulation/multi_vehicle_simulation_gazebo.html

Jaroan commented 4 years ago

@Jaeyoung-Lim would I need to copy the model as well?

The instructions at https://dev.px4.io/master/en/simulation/multi_vehicle_simulation_gazebo.html#multiple-vehicles-using-sdf-models don't say how to incorporate different sdf files in the same model. It says the SITL/Gazebo port number is automatically inserted by xmstarlet for each spawned vehicle, and does not need to be specified in the SDF file. The multi_uav_mavros_sitl_sdf.launch file calls the single_vehicle_spawn_sdf.launch https://github.com/PX4/Firmware/blob/master/launch/multi_uav_mavros_sitl_sdf.launch#L28 and vehicle is set only once https://github.com/PX4/Firmware/blob/master/launch/multi_uav_mavros_sitl_sdf.launch#L61

It also says to add a new vehicle, modify the single_vehicle_spawn_sdf.launch file to point to the location of your model by changing the line below to point to your model: $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf https://github.com/PX4/Firmware/blob/master/launch/single_vehicle_spawn_sdf.launch#L23 Wouldn't the single_vehicle_spawn_sdf.launch point to the same sdf both the times?

Jaeyoung-Lim commented 4 years ago

Right, I think you need to verify where it is broken, including your code

Jaroan commented 4 years ago

I rebuilt the latest Firmware from master. The gimbal of one vehicle out of two is controllable and follows the mount_control messages.

Jaeyoung-Lim commented 4 years ago

@Jaroan Cool, I guess you need to figure out what is wrong with instance 2. Maybe the first thing I would check is that your code is sending the control messages correctly and it reaches the firmware

swaroophangal commented 4 years ago

Hello @Jaroan ,

IMHO the only changes that would be required would be, to add the following lines to single_vehicle_spawn_sdf.launch after https://github.com/PX4/Firmware/blob/master/launch/single_vehicle_spawn_sdf.launch#L23 :

<arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;mavlink_interface&quot;]/mavlink_udp_port' -s '//plugin[@name=&quot;mavlink_interface&quot;]' -t elem -n mavlink_udp_port -v $(arg mavlink_udp_port) $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf"/> <arg name="cmd" default="xmlstarlet ed -d '//plugin[@name=&quot;GstCameraPlugin&quot;]/udpPort' -s '//plugin[@name=&quot;GstCameraPlugin&quot;]/udpPort' -t elem -n udpPort -v $(arg video_udp_port) $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf"/>

I am stuck at how do I concatenate these xmlstarlet commands before the corrected xml being sent to spawn_model.

@Jaeyoung-Lim please correct me if I am wrong.

Also, I am facing a weird issue since a few days. GPS messages are not received by PX4 SITL from gazebo. On commander gps status returns not running, while trying to start returns an error as ERROR [gps] ERR: -1 (tcsetattr) Also, all parameters requests by mavros fail.

Don't know my config issue or something else. I tried recloning Firmware multiple times.

Jaeyoung-Lim commented 4 years ago

@swaroophangal When you describe this 'weird' error since a few days, what changed in these "few days"?

swaroophangal commented 4 years ago

@Jaeyoung-Lim I tried running sitl+iris model with just gazebo and it works. Just the running with ROS part does not work. Anyways, I have opened another issue for this.. Sorry for hijacking the main issue. About the multi-vehicle sitl with ROS+Gazebo, am I right in my approach?

Jaeyoung-Lim commented 4 years ago

@swaroophangal That looks more or less correct, but you also need to stream MOUNT_ORIENTATION messages as done in https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/6011_typhoon_h480.post#L10

@Jaroan It seems that the single_vehicle_launch_sdf starts the rcS directly, making the typhoon_h480.post not run as part the startup script. (therefore no gimbal controls when launching two)

Jaroan commented 4 years ago

@swaroophangal Thank you for the suggestion! Were you able to figure out how to concatenate these commands as the arg 'cmd' has already been declared in the launch file for mavlink_tcp_port and is not allowing the additional lines? The following is the error

RLException: while processing /src/Firmware/launch/single_vehicle_spawn_sdf.launch:
Invalid <arg> tag: arg 'cmd' has already been declared.

Arg xml is <arg default="xmlstarlet ed -d '//plugin[@name=&quot;mavlink_interface&quot;]/mavlink_udp_port' -s '//plugin[@name=&quot;mavlink_interface&quot;]' -t elem -n mavlink_udp_port -v $(arg mavlink_udp_port) $(find px4)/Tools/sitl_gazebo/models/$(arg vehicle)/$(arg vehicle).sdf" name="cmd"/>

@Jaeyoung-Lim I am able to control one vehicle's (say uav0 out of uav0 and uav1) gimbal accurately. I'm unable to figure out why only one of the two. I have modified the launch file as mentioned earlier to be able to launch the two sdf files.

The single_vehicle_spawn_sdf.launch launch file was also duplicated appropriately and renamed to include the new sdf file and changes made to the multi_uav_mavros_sitl_sdf.launch file.

   <group ns="uav0">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="0"/>
        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/single_vehicle_spawn_sdf.launch">
.
.
.
    <group ns="uav1">
        <!-- MAVROS and vehicle configs -->
        <arg name="ID" value="1"/>
        <arg name="fcu_url" default="udp://:14541@localhost:14581"/>
        <!-- PX4 SITL and vehicle spawn -->
        <include file="$(find px4)/launch/second_vehicle_spawn_sdf.launch">
swaroophangal commented 4 years ago

@Jaroan , exactly the error that I ran into when I tried. Current stuck there. Once these ports have been assigned in the sdf file, I guess we might not need multiple sdf files. I tried running a simulation with two vehicles, and getting back the orientation of the mount. One peculiar issue that I observed was that the mount command was being reflected back correctly on the topic, but was not reflected in Gazebo. Most probably, there might be some issue in sitl_gazebo gimbal plugin. Somewhere, namespaces are not being honored. @Jaeyoung-Lim , could you please verify whether this is the case?

multiVehicle_gimbal_control_issue

Also, don't know how to clear up the garbled video output from Gazebo. But i can make out the landing pad in both video feeds(assuming Qgroundcontrol is switching between the feeds)

Jaroan commented 4 years ago

@swaroophangal Are any of the vehicles responding to the commands?

Jaeyoung-Lim commented 4 years ago

A working example of multiuav (without gimbal) can be found here: https://github.com/Jaeyoung-Lim/mavros_controllers/pull/137

Hope this helps

Jaeyoung-Lim commented 4 years ago

@swaroophangal Can you maybe check https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_gimbal_controller_plugin.cpp#L331 ?

The video is related to your system(hardware) and is a known issue in QGC. Maybe you should look into the threads or post a issue

@Jaroan I gave it a dig in https://github.com/Jaeyoung-Lim/mavros_humantracking/pull/9 , and I can verify that all the port numberings .etc works. You can verfiy it with the description of the PR

swaroophangal commented 4 years ago

@Jaroan Yes, both vehicles respond to position and gimbal control commands as seen from the rostopics, but not in Gazebo. That lead me to think that port wise everything is fine on PX4 end. In general multi-vehicle simulation with just vehicles(and not gimbal+camera) works quite well.

@Jaeyoung-Lim I will check out the gimbal_controller_plugin and get back to you.

I will also check out the QGC issue. I tried searching for the same, but Google failed me. Will search in QGC git. Thanks.

Jaeyoung-Lim commented 4 years ago

@Jaroan Yes, both vehicles respond to position and gimbal control commands as seen from the rostopics, but not in Gazebo. That lead me to think that port wise everything is fine on PX4 end. In general multi-vehicle simulation with just vehicles(and not gimbal+camera) works quite well.

I have verified this with the link I posted above. Seems like it is coming from the plugin

petertheprocess commented 4 years ago

@Jaroan @Jaeyoung-Lim

Hi, I guess the proplem might lies in this section https://github.com/PX4/sitl_gazebo/blob/4ec5caad7f8d4fa98ed74a6acebd2b2fb0501724/models/typhoon_h480/typhoon_h480.sdf#L1428

<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>

when two vehicles are spawned in gazebo, only the first instance is called 'typhoon_h480', the second one might be 'typhoon_h480_1', so the plugin cannot find the correct joint to control.

So delete this namespace 'typhoon_h480' can solve this issue.

<plugin name='gimbal_controller' filename='libgazebo_gimbal_controller_plugin.so'>
      <joint_yaw>cgo3_vertical_arm_joint</joint_yaw>
      <joint_roll>cgo3_horizontal_arm_joint</joint_roll>
      <joint_pitch>cgo3_camera_joint</joint_pitch>
Jaeyoung-Lim commented 4 years ago

@petertheprocess That was also my suspicion, but didnt work for me. Did you have any success?

Jaroan commented 4 years ago

@petertheprocess Removing the namespaces didn't work for me as well.

swaroophangal commented 4 years ago

@Jaeyoung-Lim Some observations:

  1. Values of angle command, angle error and angle direction for all joints are logical and change alongwith commands from ROS. All these values are similar for all vehicles given similar commands.
  2. While applying those forces at https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_gimbal_controller_plugin.cpp#L601, calculated force values are going wonky.
  3. Observation 2 can be seen on random vehicles(in a set of two vehicles, I saw this randomly on both but not at the same time)

Also, i did away with fixed namespaces(typhoon_h480) in the sdf file defined for sonar plugin and all gimbal joints. The above observations are valid for both with and without the namespaces.

petertheprocess commented 4 years ago

@petertheprocess That was also my suspicion, but didnt work for me. Did you have any success? @petertheprocess Removing the namespaces didn't work for me as well.

@Jaroan @Jaeyoung-Lim @swaroophangal Sorry, removing the namesapce does not worlk, I failed too.

But when I duplicate a new typhoon_h480_1.sdf like that

<sdf version='1.5'>
  <model name='typhoon_h480_1'>
...........
..........
........
      <sensor name="camera_imu_1" type="imu">
    <plugin name='gimbal_controller' filename='libgazebo_gimbal_controller_plugin.so'>
      <joint_yaw>typhoon_h480_1::cgo3_vertical_arm_joint</joint_yaw>
      <joint_roll>typhoon_h480_1::cgo3_horizontal_arm_joint</joint_roll>
      <joint_pitch>typhoon_h480_1::cgo3_camera_joint</joint_pitch>
 ..........
........
      <gimbal_imu>camera_imu_1</gimbal_imu>
    </plugin>
  </model>
</sdf>

I succed! And if I don't change the name, the gimbal of typhoon_h480_1 can respons the mavros control cmd, but the orientation seems wrong.

BUT What confused me is that in the https://github.com/PX4/sitl_gazebo/blob/master/src/gazebo_gimbal_controller_plugin.cpp forces are applied on the joints, and the JointPtr is set by

this->yawJoint = this->model->GetJoint(yawJointName);

and the model in the upper code section is a ModelPtr, which is set by gazebo plugin API (I am not sure.).

void GimbalControllerPlugin::Load(physics::ModelPtr _model,
  sdf::ElementPtr _sdf)
{
  this->model = _model;
..............
................
................
}

If each model instance in gazebo has a unique ModelPtr，then there is no difference between this->model->GetJoint('cgo3_vertical_arm_joint') and this->model->GetJoint('typhoon_h480_1::cgo3_vertical_arm_joint'). So, there is no need to duplicate a new sdf, just remove the namespace could work. But we all faied. Is there anybody can explain the plugin code?

Jaeyoung-Lim commented 4 years ago

@petertheprocess what do you mean by success? Were you able to control both gimbals separately?

petertheprocess commented 4 years ago

@petertheprocess what do you mean by success? Were you able to control both gimbals separately?

Yep.

petertheprocess commented 4 years ago

And in http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/group__gazebo__sensors.html, I find some description about the function GetSensor().

SensorPtr gazebo::sensors::get_sensor ( const std::string & _name )  

This name should be fully scoped. This means _name = world_name::model_name::link_name::sensor_name. You may use the unscoped sensor name if that name is unique within the entire simulation. If the name is not unique a nullptr pointer is returned.

Though in https://github.com/PX4/sitl_gazebo/blob/629e5f5c239d88421edab12b4a7e2e3c3ea7e8fc/src/gazebo_gimbal_controller_plugin.cpp#L338 GetSensor() was used, I think the description might work for it too. So does GetJoint().

Jaeyoung-Lim commented 4 years ago

@petertheprocess @Jaroan I have fixed the issue in #549 Can you check if this solves the issue?

Jaroan commented 4 years ago

@Jaeyoung-Lim I wasn't able to control both gimbals using the fix in #549. I don't understand why though. Should I rebuild after incorporating the changes in the src/gazebo_gimbal_controller_plugin.cpp file?

However, using @petertheprocess's method of using the second sdf file and renaming the model instances as mentioned worked.

........... .......... ........ typhoon_h480_1::cgo3_vertical_arm_joint typhoon_h480_1::cgo3_horizontal_arm_joint typhoon_h480_1::cgo3_camera_joint .......... ........ camera_imu_1
Jaeyoung-Lim commented 4 years ago

@Jaroan of course you need to rebuild with the fix

Jaroan commented 4 years ago

@Jaeyoung-Lim Yes, this fixed the issue. Thank you very much for your support throughout!

GuangyaoSHI commented 2 years ago

@Jaeyoung-Lim Yes, this fixed the issue. Thank you very much for your support throughout!

How do you fix the issue? Rebuild with the fix or use the second sdf? I tried the latest PX4 in which the #549 should be fixed. But I can still only control one gimbal.

GuangyaoSHI commented 2 years ago

I am now using V1.12.1 PX4 and I notice that in the gimbal controller plugin, the camera_imu bug is fixed. But when I launch two typhoon_h480, only the gimbal of the first drone is controllable. The /uav0/mount_control command and /uav1/mout_control command are properly send, which can be observed in the rostopic. I notice that when the PX4 for uav1 is launching, it prompts a warning that bind failed, address already in use. I used the default launch file for multiple drones. So the address should be proper assigned. I am quite confused why the gimbal of uav1 is not controllable.

INFO [ekf2] starting instance 4, IMU:1 (1310996), MAG:1 (197644) INFO [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550 INFO [ekf2] starting instance 5, IMU:2 (1311004), MAG:1 (197644) INFO [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030 INFO [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280 INFO [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14558 remote port 14530 WARN [mavlink] bind failed: Address already in use

Jaeyoung-Lim commented 2 years ago

@GuangyaoSHI You can find the example in https://github.com/Jaeyoung-Lim/mavros_humantracking

GuangyaoSHI commented 2 years ago

@GuangyaoSHI You can find the example in https://github.com/Jaeyoung-Lim/mavros_humantracking

Thank you for your response. I tested this example in my desktop. I didn't make any changes to either this ros package (dependencies are installed ) or PX4 (v1.12.1). However, when I launch multiple drone example, two drones can take off but they cannot follow the set trajectory. They will soon drop to the ground. Do I need tune any parameters?

I noticed that despite they can fly, actually only one gimbal is moving.

So I did another test. I deleted the geometric controller node because the gimbal control is independent of pose control. I observed the same phenomenon. uav0's gimbal will change its orientation but uav1 will only look at its default direction.

Do you have any idea what could go wrong? Thank you for your help.

I observed the following warning information when I launched the file. WARN [mavlink] bind failed: Address already in use

Jaeyoung-Lim commented 2 years ago

@GuangyaoSHI Could you post a new issue on https://github.com/Jaeyoung-Lim/mavros_humantracking with the instructions on how to reproduce your issue? (how you are starting the simulation etc)

GuangyaoSHI commented 2 years ago

@GuangyaoSHI Could you post a new issue on https://github.com/Jaeyoung-Lim/mavros_humantracking with the instructions on how to reproduce your issue? (how you are starting the simulation etc)

Thank you for your reply. I solved the problem of controlling multiple gimbals at the same time. I found another open-source project XTdrone, which is also based on PX4. They re-write the gimbal controller plugin and I can use their version of PX4 to control multiple gimbals.

Here is the project page. On this page, by executing the command line in the PX4 configuration section, I switched to their modified v1.11.0-beta1 version. Then follow the instructions in the XTDrone Source Code Download section to copy some files to the PX4.

Then, I used multi_vehicle. launch file to launch multiple typhoon_h480 and successfully controlled each gimbal.

Jaeyoung-Lim commented 2 years ago

@GuangyaoSHI I don't think reverting to an older PX4 version is a solution.