forked from Archive/PX4-Autopilot
simulation/gz_bridge: rc_cessna plane model working, Gazebo Garden updates, and prepare for proper airspeed (#20989)
Co-authored-by: Benjamin Perseghetti <bperseghetti@rudislabs.com> Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
This commit is contained in:
Benjamin Perseghetti
GitHub
parent
fbd2e111d0
commit
684b4a4b8a
|
|
@ -1,6 +1,6 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# @name Gazebo plane
|
||||
# @name Gazebo rc_cessna
|
||||
# @type Fixedwing
|
||||
#
|
||||
|
||||
|
|
@ -8,7 +8,7 @@
|
|||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=plane}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna}
|
||||
|
||||
param set-default EKF2_MAG_ACCLIM 0
|
||||
param set-default EKF2_MAG_YAWLIM 0
|
||||
|
|
@ -72,7 +72,7 @@ px4_add_romfs_files(
|
|||
|
||||
4001_gz_x500
|
||||
4002_gz_x500_depth
|
||||
4003_gz_plane
|
||||
4003_gz_rc_cessna
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
|
|
|||
|
|
@ -21,7 +21,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
|
|||
|
||||
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
|
||||
|
||||
# source generated gz_env.sh for IGN_GAZEBO_RESOURCE_PATH
|
||||
# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
|
||||
if [ -f ./gz_env.sh ]; then
|
||||
. ./gz_env.sh
|
||||
|
||||
|
|
@ -37,16 +37,8 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
|
|||
gz_command="gz"
|
||||
gz_sub_command="sim"
|
||||
else
|
||||
IGN_GAZEBO_VERSIONS=$(ign gazebo --versions 2>&1)
|
||||
if [ $? -eq 0 ] && [ "${IGN_GAZEBO_VERSIONS}" != "" ]
|
||||
then
|
||||
# "ign gazebo" for Fortress and earlier
|
||||
gz_command="ign"
|
||||
gz_sub_command="gazebo"
|
||||
else
|
||||
echo "ERROR [init] Gazebo gz and ign commands unavailable"
|
||||
exit 1
|
||||
fi
|
||||
echo "ERROR [init] Gazebo gz please install gz-garden"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# look for running ${gz_command} gazebo world
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
<?xml version="1.0"?>
|
||||
<model>
|
||||
<name>Plane</name>
|
||||
<name>rc_cessna</name>
|
||||
<version>1.0</version>
|
||||
<sdf version='1.9'>model.sdf</sdf>
|
||||
|
||||
|
|
@ -10,6 +10,6 @@
|
|||
</author>
|
||||
|
||||
<description>
|
||||
This is a model of a standard plane.
|
||||
This is a model of an RC Cessna 182.
|
||||
</description>
|
||||
</model>
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
<?xml version="1.0"?>
|
||||
<sdf version='1.9'>
|
||||
<model name='plane'>
|
||||
<model name='rc_cessna'>
|
||||
<pose>0 0 0.246 0 0 0</pose>
|
||||
<link name='base_link'>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
|
|
@ -16,11 +16,11 @@
|
|||
<izz>0.1477</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='base_link_collision'>
|
||||
<pose>0 0 -0.07 0 0 0</pose>
|
||||
<collision name='fuselodge_collision'>
|
||||
<pose>-.14 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.47 0.47 0.11</size>
|
||||
<size>0.65 .08 0.10</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
|
|
@ -32,12 +32,44 @@
|
|||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<collision name='wings_collision'>
|
||||
<pose>-0.01 0 0.07 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 1.0 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<max_vel>10</max_vel>
|
||||
<min_depth>0.01</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<!-- <visual name='fuselodge_collision_visual'>
|
||||
<pose>-.14 0 0 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.65 .08 0.1</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</visual>
|
||||
<visual name='wings_collision_visual'>
|
||||
<pose>-0.01 0 0.07 0 0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.1 1.0 0.01</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</visual> -->
|
||||
<visual name='base_link_visual'>
|
||||
<pose>0.07 0 -0.08 0 0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/body.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/body.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -67,7 +99,7 @@
|
|||
<izz>1e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<visual name="visual">
|
||||
<visual name="airspeed_visual">
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<radius>0.01</radius>
|
||||
|
|
@ -98,7 +130,7 @@
|
|||
<parent>base_link</parent>
|
||||
</joint>
|
||||
<link name='rotor_puller'>
|
||||
<pose>0.22 0 0.0 0 1.57 0</pose>
|
||||
<pose>0.22 0 0.0 0 1.57079632679 0</pose>
|
||||
<inertial>
|
||||
<pose>0 0 0 0 0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
|
|
@ -112,7 +144,7 @@
|
|||
</inertia>
|
||||
</inertial>
|
||||
<collision name='rotor_puller_collision'>
|
||||
<pose>0.0 0 0 0 0 0</pose>
|
||||
<pose>0 0 0 0 -1.57079632679 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.005 0.22 0.02</size>
|
||||
|
|
@ -128,7 +160,7 @@
|
|||
</surface>
|
||||
</collision>
|
||||
<!-- <visual name='rotor_puller_collision_visual'>
|
||||
<pose>0.0 0 0 0 0 0</pose>
|
||||
<pose>0 0 0 0 -1.57079632679 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.005 0.22 0.02</size>
|
||||
|
|
@ -136,11 +168,11 @@
|
|||
</geometry>
|
||||
</visual> -->
|
||||
<visual name='rotor_puller_visual'>
|
||||
<pose>0 0 0 0.0 0 0.0</pose>
|
||||
<pose>0 0 0 0 0 -1.57079632679</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://plane/meshes/iris_prop_ccw.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -186,7 +218,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/left_aileron.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/left_aileron.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -195,6 +227,135 @@
|
|||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="LeftWheel">
|
||||
<pose relative_to="LeftWheelJoint">0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>.05</mass>
|
||||
<inertia>
|
||||
<ixx>0.00003331</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0000204</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0000204</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<gravity>true</gravity>
|
||||
<velocity_decay/>
|
||||
<visual name="LeftWheelVisual">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.03</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="LeftWheelCollision">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.03</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>0.5</mu2>
|
||||
<fdir1>0 0 1</fdir1>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="RightWheel">
|
||||
<pose relative_to="RightWheelJoint">0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>.05</mass>
|
||||
<inertia>
|
||||
<ixx>0.00003331</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0000204</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0000204</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<gravity>true</gravity>
|
||||
<velocity_decay/>
|
||||
<visual name="RightWheelVisual">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.03</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="RightWheelCollision">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.03</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>0.5</mu2>
|
||||
<fdir1>0 0 1</fdir1>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="CenterWheel">
|
||||
<pose relative_to="CenterWheelJoint">0 0 0 0 0 0</pose>
|
||||
<inertial>
|
||||
<mass>.05</mass>
|
||||
<inertia>
|
||||
<ixx>0.00003331</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.0000204</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0000204</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<gravity>true</gravity>
|
||||
<velocity_decay/>
|
||||
<visual name="CenterWheelVisual">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.025</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="CenterWheelCollision">
|
||||
<pose>0 0 0 -1.57079632679 0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.01</length>
|
||||
<radius>0.025</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1.0</mu>
|
||||
<mu2>0.5</mu2>
|
||||
<fdir1>0 0 1</fdir1>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="right_elevon">
|
||||
<inertial>
|
||||
<mass>0.00000001</mass>
|
||||
|
|
@ -213,7 +374,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/right_aileron.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/right_aileron.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -240,7 +401,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/left_flap.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/left_flap.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -267,7 +428,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/right_flap.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/right_flap.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -294,7 +455,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/elevators.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/elevators.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -321,7 +482,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://plane/meshes/rudder.dae</uri>
|
||||
<uri>model://rc_cessna/meshes/rudder.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
@ -451,6 +612,57 @@
|
|||
</ode>
|
||||
</physics>
|
||||
</joint>
|
||||
<joint name="LeftWheelJoint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>LeftWheel</child>
|
||||
<pose relative_to="base_link">-.035 .13 -0.12 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1.79769e+308</lower>
|
||||
<upper>1.79769e+308</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<joint name="RightWheelJoint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>RightWheel</child>
|
||||
<pose relative_to="base_link">-.035 -.13 -0.12 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1.79769e+308</lower>
|
||||
<upper>1.79769e+308</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<joint name="CenterWheelJoint" type="revolute">
|
||||
<parent>base_link</parent>
|
||||
<child>CenterWheel</child>
|
||||
<pose relative_to="base_link">.135 0 -0.12 0 0 0</pose>
|
||||
<axis>
|
||||
<xyz>0 1 0</xyz>
|
||||
<limit>
|
||||
<lower>-1.79769e+308</lower>
|
||||
<upper>1.79769e+308</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
<cla>4.752798721</cla>
|
||||
|
|
@ -472,6 +684,7 @@
|
|||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>px4_servo_0</joint_name>
|
||||
<sub_topic>px4_servo_0</sub_topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
|
|
@ -494,6 +707,7 @@
|
|||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>px4_servo_1</joint_name>
|
||||
<sub_topic>px4_servo_1</sub_topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.05984281113</a0>
|
||||
|
|
@ -531,7 +745,7 @@
|
|||
<control_joint_name>right_flap_joint</control_joint_name>
|
||||
<control_joint_rad_to_cl>-0.1</control_joint_rad_to_cl>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>-0.2</a0>
|
||||
<cla>4.752798721</cla>
|
||||
<cda>0.6417112299</cda>
|
||||
|
|
@ -552,6 +766,7 @@
|
|||
<plugin
|
||||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>px4_servo_2</joint_name>
|
||||
<sub_topic>px4_servo_2</sub_topic>
|
||||
</plugin>
|
||||
<plugin filename="gz-sim-lift-drag-system" name="gz::sim::systems::LiftDrag">
|
||||
<a0>0.0</a0>
|
||||
|
|
@ -575,7 +790,7 @@
|
|||
filename="gz-sim-joint-position-controller-system" name="gz::sim::systems::JointPositionController">
|
||||
<joint_name>rudder_joint</joint_name>
|
||||
</plugin>
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_puller_joint</jointName>
|
||||
<linkName>rotor_puller</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
|
|
@ -585,7 +800,7 @@
|
|||
<motorConstant>2.44858e-05</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>command/motor_speed</commandSubTopic>
|
||||
<motorNumber>3</motorNumber>
|
||||
<motorNumber>0</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
|
|
@ -528,7 +528,7 @@
|
|||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_1_joint</jointName>
|
||||
<linkName>rotor_1</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
|
|
@ -544,7 +544,7 @@
|
|||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_2_joint</jointName>
|
||||
<linkName>rotor_2</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
|
|
@ -560,7 +560,7 @@
|
|||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin filename="ignition-gazebo-multicopter-motor-model-system" name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
|
||||
<jointName>rotor_3_joint</jointName>
|
||||
<linkName>rotor_3</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
|
|
|
|||
|
|
@ -35,9 +35,7 @@
|
|||
find_package(gz-transport
|
||||
#REQUIRED COMPONENTS core
|
||||
NAMES
|
||||
ignition-transport8
|
||||
ignition-transport10
|
||||
ignition-transport11
|
||||
#ignition-transport11 # IGN (Fortress and earlier) no longer supported
|
||||
gz-transport12
|
||||
#QUIET
|
||||
)
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@
|
|||
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
|
|
@ -70,8 +71,7 @@ int GZBridge::init()
|
|||
if (!_model_sim.empty()) {
|
||||
|
||||
// service call to create model
|
||||
// ign service -s /world/${PX4_GZ_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_GZ_MODEL}/model.sdf\""
|
||||
ignition::msgs::EntityFactory req{};
|
||||
gz::msgs::EntityFactory req{};
|
||||
req.set_sdf_filename(_model_sim + "/model.sdf");
|
||||
|
||||
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
|
||||
|
|
@ -95,16 +95,16 @@ int GZBridge::init()
|
|||
model_pose_v.push_back(0.0);
|
||||
}
|
||||
|
||||
ignition::msgs::Pose *p = req.mutable_pose();
|
||||
ignition::msgs::Vector3d *position = p->mutable_position();
|
||||
gz::msgs::Pose *p = req.mutable_pose();
|
||||
gz::msgs::Vector3d *position = p->mutable_position();
|
||||
position->set_x(model_pose_v[0]);
|
||||
position->set_y(model_pose_v[1]);
|
||||
position->set_z(model_pose_v[2]);
|
||||
|
||||
ignition::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
|
||||
gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]);
|
||||
|
||||
q.Normalize();
|
||||
ignition::msgs::Quaternion *orientation = p->mutable_orientation();
|
||||
gz::msgs::Quaternion *orientation = p->mutable_orientation();
|
||||
orientation->set_x(q.X());
|
||||
orientation->set_y(q.Y());
|
||||
orientation->set_z(q.Z());
|
||||
|
|
@ -112,7 +112,7 @@ int GZBridge::init()
|
|||
}
|
||||
|
||||
//world/$WORLD/create service.
|
||||
ignition::msgs::Boolean rep;
|
||||
gz::msgs::Boolean rep;
|
||||
bool result;
|
||||
std::string create_service = "/world/" + _world_name + "/create";
|
||||
|
||||
|
|
@ -152,6 +152,18 @@ int GZBridge::init()
|
|||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
#if 0
|
||||
// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
|
||||
std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name +
|
||||
"/link/airspeed_link/sensor/air_speed/air_speed";
|
||||
|
||||
if (!_node.Subscribe(airpressure_topic, &GZBridge::airpressureCallback, this)) {
|
||||
PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str());
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (!_mixing_interface_esc.init(_model_name)) {
|
||||
PX4_ERR("failed to init ESC output");
|
||||
return PX4_ERROR;
|
||||
|
|
@ -304,7 +316,7 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec)
|
|||
return false;
|
||||
}
|
||||
|
||||
void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
|
||||
void GZBridge::clockCallback(const gz::msgs::Clock &clock)
|
||||
{
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
|
|
@ -317,7 +329,33 @@ void GZBridge::clockCallback(const ignition::msgs::Clock &clock)
|
|||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
|
||||
#if 0
|
||||
void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure)
|
||||
{
|
||||
if (hrt_absolute_time() == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&_mutex);
|
||||
|
||||
const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000)
|
||||
+ (air_pressure.header().stamp().nsec() / 1000);
|
||||
|
||||
double air_pressure_value = air_pressure.pressure();
|
||||
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = time_us;
|
||||
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
|
||||
report.differential_pressure_pa = static_cast<float>(air_pressure_value); // hPa to Pa;
|
||||
report.temperature = static_cast<float>(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
|
||||
report.timestamp = hrt_absolute_time();;
|
||||
_differential_pressure_pub.publish(report);
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
#endif
|
||||
|
||||
void GZBridge::imuCallback(const gz::msgs::IMU &imu)
|
||||
{
|
||||
if (hrt_absolute_time() == 0) {
|
||||
return;
|
||||
|
|
@ -332,12 +370,12 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
|
|||
}
|
||||
|
||||
// FLU -> FRD
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0);
|
||||
|
||||
ignition::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
|
||||
imu.linear_acceleration().x(),
|
||||
imu.linear_acceleration().y(),
|
||||
imu.linear_acceleration().z()));
|
||||
gz::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
|
||||
imu.linear_acceleration().x(),
|
||||
imu.linear_acceleration().y(),
|
||||
imu.linear_acceleration().z()));
|
||||
|
||||
// publish accel
|
||||
sensor_accel_s sensor_accel{};
|
||||
|
|
@ -357,10 +395,10 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
|
|||
_sensor_accel_pub.publish(sensor_accel);
|
||||
|
||||
|
||||
ignition::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(ignition::math::Vector3d(
|
||||
imu.angular_velocity().x(),
|
||||
imu.angular_velocity().y(),
|
||||
imu.angular_velocity().z()));
|
||||
gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d(
|
||||
imu.angular_velocity().x(),
|
||||
imu.angular_velocity().y(),
|
||||
imu.angular_velocity().z()));
|
||||
|
||||
// publish gyro
|
||||
sensor_gyro_s sensor_gyro{};
|
||||
|
|
@ -382,7 +420,7 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
|
|||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
|
||||
{
|
||||
if (hrt_absolute_time() == 0) {
|
||||
return;
|
||||
|
|
@ -402,10 +440,10 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
|||
const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1);
|
||||
_timestamp_prev = time_us;
|
||||
|
||||
ignition::msgs::Vector3d pose_position = pose.pose(p).position();
|
||||
ignition::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
|
||||
gz::msgs::Vector3d pose_position = pose.pose(p).position();
|
||||
gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation();
|
||||
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0);
|
||||
|
||||
/**
|
||||
* @brief Quaternion for rotation between ENU and NED frames
|
||||
|
|
@ -414,17 +452,17 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
|
|||
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
|
||||
* This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
|
||||
*/
|
||||
static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
|
||||
static const auto q_ENU_to_NED = gz::math::Quaterniond(0, 0.70711, 0.70711, 0);
|
||||
|
||||
// ground truth
|
||||
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
|
||||
pose_orientation.w(),
|
||||
pose_orientation.x(),
|
||||
pose_orientation.y(),
|
||||
pose_orientation.z());
|
||||
gz::math::Quaterniond q_gr = gz::math::Quaterniond(
|
||||
pose_orientation.w(),
|
||||
pose_orientation.x(),
|
||||
pose_orientation.y(),
|
||||
pose_orientation.z());
|
||||
|
||||
ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
|
||||
ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
|
||||
gz::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
|
||||
gz::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
|
||||
|
||||
// publish attitude groundtruth
|
||||
vehicle_attitude_s vehicle_attitude_groundtruth{};
|
||||
|
|
|
|||
|
|
@ -47,6 +47,7 @@
|
|||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <uORB/topics/sensor_accel.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
|
|
@ -54,10 +55,12 @@
|
|||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
||||
#include <ignition/msgs.hh>
|
||||
#include <ignition/transport.hh>
|
||||
#include <ignition/math.hh>
|
||||
#include <ignition/msgs/imu.pb.h>
|
||||
#include <gz/math.hh>
|
||||
#include <gz/msgs.hh>
|
||||
#include <gz/transport.hh>
|
||||
|
||||
// #include <gz/msgs/fluid_pressure.pb.h>
|
||||
#include <gz/msgs/imu.pb.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
|
@ -89,13 +92,16 @@ private:
|
|||
|
||||
bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec);
|
||||
|
||||
void clockCallback(const ignition::msgs::Clock &clock);
|
||||
void imuCallback(const ignition::msgs::IMU &imu);
|
||||
void poseInfoCallback(const ignition::msgs::Pose_V &pose);
|
||||
void clockCallback(const gz::msgs::Clock &clock);
|
||||
|
||||
//void airpressureCallback(const gz::msgs::FluidPressure &air_pressure);
|
||||
void imuCallback(const gz::msgs::IMU &imu);
|
||||
void poseInfoCallback(const gz::msgs::Pose_V &pose);
|
||||
|
||||
// Subscriptions
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
//uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
|
||||
uORB::Publication<vehicle_attitude_s> _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
|
||||
uORB::Publication<vehicle_global_position_s> _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
|
||||
|
|
@ -123,7 +129,7 @@ private:
|
|||
const std::string _model_sim;
|
||||
const std::string _model_pose;
|
||||
|
||||
ignition::transport::Node _node;
|
||||
gz::transport::Node _node;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
|
|||
|
||||
// output eg /X500/command/motor_speed
|
||||
std::string actuator_topic = "/" + model_name + "/command/motor_speed";
|
||||
_actuators_pub = _node.Advertise<ignition::msgs::Actuators>(actuator_topic);
|
||||
_actuators_pub = _node.Advertise<gz::msgs::Actuators>(actuator_topic);
|
||||
|
||||
if (!_actuators_pub.Valid()) {
|
||||
PX4_ERR("failed to advertise %s", actuator_topic.c_str());
|
||||
|
|
@ -73,7 +73,7 @@ bool GZMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_
|
|||
}
|
||||
|
||||
if (active_output_count > 0) {
|
||||
ignition::msgs::Actuators rotor_velocity_message;
|
||||
gz::msgs::Actuators rotor_velocity_message;
|
||||
rotor_velocity_message.mutable_velocity()->Resize(active_output_count, 0);
|
||||
|
||||
for (unsigned i = 0; i < active_output_count; i++) {
|
||||
|
|
@ -96,7 +96,7 @@ void GZMixingInterfaceESC::Run()
|
|||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void GZMixingInterfaceESC::motorSpeedCallback(const ignition::msgs::Actuators &actuators)
|
||||
void GZMixingInterfaceESC::motorSpeedCallback(const gz::msgs::Actuators &actuators)
|
||||
{
|
||||
if (hrt_absolute_time() == 0) {
|
||||
return;
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@
|
|||
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
|
||||
#include <ignition/transport.hh>
|
||||
#include <gz/transport.hh>
|
||||
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
|
@ -49,7 +49,7 @@ class GZMixingInterfaceESC : public OutputModuleInterface
|
|||
public:
|
||||
static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
|
||||
|
||||
GZMixingInterfaceESC(ignition::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
|
|
@ -73,14 +73,14 @@ private:
|
|||
|
||||
void Run() override;
|
||||
|
||||
void motorSpeedCallback(const ignition::msgs::Actuators &actuators);
|
||||
void motorSpeedCallback(const gz::msgs::Actuators &actuators);
|
||||
|
||||
ignition::transport::Node &_node;
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
ignition::transport::Node::Publisher _actuators_pub;
|
||||
gz::transport::Node::Publisher _actuators_pub;
|
||||
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
|
||||
|
|
|
|||
|
|
@ -35,12 +35,12 @@
|
|||
|
||||
bool GZMixingInterfaceServo::init(const std::string &model_name)
|
||||
{
|
||||
// /model/plane_0/joint/left_elevon_joint/0/cmd_pos
|
||||
// /model/rascal_110_0/px4_servo_2
|
||||
for (int i = 0; i < 8; i++) {
|
||||
std::string joint_name = "px4_servo_" + std::to_string(i);
|
||||
std::string servo_topic = "/model/" + model_name + "/joint/" + joint_name + "/0/cmd_pos";
|
||||
std::string servo_topic = "/model/" + model_name + "/" + joint_name;
|
||||
//std::cout << "Servo topic: " << servo_topic << std::endl;
|
||||
_servos_pub.push_back(_node.Advertise<ignition::msgs::Double>(servo_topic));
|
||||
_servos_pub.push_back(_node.Advertise<gz::msgs::Double>(servo_topic));
|
||||
|
||||
if (!_servos_pub.back().Valid()) {
|
||||
PX4_ERR("failed to advertise %s", servo_topic.c_str());
|
||||
|
|
@ -63,7 +63,7 @@ bool GZMixingInterfaceServo::updateOutputs(bool stop_motors, uint16_t outputs[MA
|
|||
|
||||
for (auto &servo_pub : _servos_pub) {
|
||||
if (_mixing_output.isFunctionSet(i)) {
|
||||
ignition::msgs::Double servo_output;
|
||||
gz::msgs::Double servo_output;
|
||||
///TODO: Normalize output data
|
||||
double output = (outputs[i] - 500) / 500.0;
|
||||
// std::cout << "outputs[" << i << "]: " << outputs[i] << std::endl;
|
||||
|
|
|
|||
|
|
@ -35,7 +35,7 @@
|
|||
|
||||
#include <lib/mixer_module/mixer_module.hpp>
|
||||
|
||||
#include <ignition/transport.hh>
|
||||
#include <gz/transport.hh>
|
||||
|
||||
// GZBridge mixing class for Servos.
|
||||
// It is separate from GZBridge to have separate WorkItems and therefore allowing independent scheduling
|
||||
|
|
@ -43,7 +43,7 @@
|
|||
class GZMixingInterfaceServo : public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
GZMixingInterfaceServo(ignition::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
|
||||
OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl),
|
||||
_node(node),
|
||||
_node_mutex(node_mutex)
|
||||
|
|
@ -67,10 +67,10 @@ private:
|
|||
|
||||
void Run() override;
|
||||
|
||||
ignition::transport::Node &_node;
|
||||
gz::transport::Node &_node;
|
||||
pthread_mutex_t &_node_mutex;
|
||||
|
||||
MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
std::vector<ignition::transport::Node::Publisher> _servos_pub;
|
||||
std::vector<gz::transport::Node::Publisher> _servos_pub;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -3,7 +3,4 @@
|
|||
export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models
|
||||
export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds
|
||||
|
||||
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
|
||||
|
||||
# IGN -> GZ as of Garden
|
||||
export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS
|
||||
|
|
|
|||
|
|
@ -1,5 +1,7 @@
|
|||
|
||||
find_package(gazebo)
|
||||
find_package(gazebo
|
||||
QUIET
|
||||
)
|
||||
|
||||
if(gazebo_FOUND)
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue