Hello all,
I am using the PX4-multiple Vehicles link with SDF files, and it works fine with iris model, however, I want to have multiple iris_stereo_camera models.
To be able to get connected to the QGC and update the ports from iris file with the jinja_gen.py plugin, I have combined the iris model with iris_stereo_camera model (adding the stereo_camera part to the iris model). It will connect fine to the qgc, but there are no cameras!
Moreover, it is weird that it seems two directories are reading the models, and changing in each one will affect the model:
First Directory: ~/PX4/sitl_gazebo/models
Second Directory: ~/Firmware/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models
The other thing is that sometimes when I try to change the names in the SDF file, I can see that in running topics with the name drone1/drone1/... will pop up which I have no idea from where they are published!
My launch file for the second drone is as follows:
<group ns="drone2">
<arg name="ns" value="drone2"/>
<arg name="ID" value="1"/>
<arg name="vehicle" default="iris_obs_avoid"/>
<arg name="model" default="iris_drone2"/>
<arg name="fcu_url" value="udp://:14541@localhost:14581"/>
<arg name="gcs_url" value="" /> <!-- GCS link is provided by SITL -->
<arg name="tgt_system" value="$(eval 1 + arg('ID'))" />
<arg name="tgt_component" value="1" />
<arg name="x" value="0"/>
<arg name="y" value="1"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="est" default="ekf2"/>
<env name="PX4_SIM_MODEL" value="gazebo-classic_$(arg vehicle)" />
<!-- <env name="PX4_SIM_MODEL" value="gazebo-classic_iris_obs_avoid" /> -->
<arg name="mavlink_udp_port" value="14561"/>
<arg name="mavlink_tcp_port" value="4561"/>
<arg name="gst_udp_port" value="$(eval 5600 + arg('ID'))"/>
<arg name="video_uri" value="$(eval 5600 + arg('ID'))"/>
<arg name="mavlink_cam_udp_port" value="$(eval 14530 + arg('ID'))"/>
<arg name="mavlink_id" value="$(eval 1 + arg('ID'))" />
<!-- Define a static transform from a camera internal frame to the fcu for every camera used -->
<node pkg="tf" type="static_transform_publisher" name="tf_depth_camera"
args="0 0 0 -1.57 0 -1.57 fcu camera_link 10"/>
<!-- Launch rqt_reconfigure -->
<node pkg="rqt_reconfigure" type="rqt_reconfigure" output="screen" name="rqt_reconfigure" />
<!-- PX4 configs -->
<arg name="interactive" default="true"/>
<!-- generate sdf vehicle model -->
<arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(arg mavlink_id) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg model)/$(arg model).sdf.jinja $(find mavlink_sitl_gazebo)"/>
<param command="$(arg cmd)" name="model_description"/>
<!-- PX4 SITL -->
<arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
<arg if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
<node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/build/px4_sitl_default/etc -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
</node>
<!-- spawn vehicle -->
<node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-sdf -param model_description -model $(arg vehicle)$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
<!-- Launch MavROS -->
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
<!-- Need to change the config file to get the tf topic and get local position in terms of local origin -->
<arg name="config_yaml" value="$(find avoidance)/resource/px4_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
</include>
</group>
And the iris_drone2 is just iris model with the following added, and the model name was updated:
<include>
<uri>model://stereo_camera_drone2</uri>
<pose>0.1 0 0 0 0 0</pose>
</include>
<joint name="stereo_camera_joint_drone2" type="revolute">
<child>stereo_camera_drone2::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
I have updated the last part of the stereo_camera as well, and updated the model names:
<plugin name="stereo_camera_controller_drone2" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>drone2/stereo</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>camera_link</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
I would appreciate you help and tips.