dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1853 from PX4/ros_mavlink_rotorssimulatorupdate 

update ros launch files and nodes for update of rotors_simulator
This commit is contained in:
Thomas Gubler 2015-03-10 21:26:56 +01:00
parent 1837440e43 b311f7302b
commit f2b2c55afd
20 changed files with 115 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
integrationtests/demo_tests/direct_manual_input_test.py
View File

@ -64,8 +64,8 @@ class ManualInputTest(unittest.TestCase):
# #
def test_manual_input(self): def test_manual_input(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) rospy.Subscriber('iris/actuator_armed', actuator_armed, self.actuator_armed_callback)
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
man = ManualInput() man = ManualInput()

6
integrationtests/demo_tests/direct_offboard_posctl_test.py
View File

@ -66,9 +66,9 @@ class DirectOffboardPosctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback)
self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
self.rate = rospy.Rate(10) # 10hz self.rate = rospy.Rate(10) # 10hz
def tearDown(self): def tearDown(self):

4
integrationtests/demo_tests/manual_input.py
View File

@ -54,8 +54,8 @@ class ManualInput:
def __init__(self): def __init__(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) self.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10)
self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) self.pubOff = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10)
self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
def arm(self): def arm(self):

12
integrationtests/demo_tests/mavros_offboard_attctl_test.py
View File

@ -65,12 +65,12 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
rospy.wait_for_service('mavros/cmd/arming', 30) rospy.wait_for_service('iris/mavros/cmd/arming', 30)
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback)
self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10)
self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10)
self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool)
self.rate = rospy.Rate(10) # 10hz self.rate = rospy.Rate(10) # 10hz
self.rateSec = rospy.Rate(1) self.rateSec = rospy.Rate(1)
self.hasPos = False self.hasPos = False

10
integrationtests/demo_tests/mavros_offboard_posctl_test.py
View File

@ -64,11 +64,11 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
def setUp(self): def setUp(self):
rospy.init_node('test_node', anonymous=True) rospy.init_node('test_node', anonymous=True)
rospy.wait_for_service('mavros/cmd/arming', 30) rospy.wait_for_service('iris/mavros/cmd/arming', 30)
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback)
self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10)
self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool)
self.rate = rospy.Rate(10) # 10hz self.rate = rospy.Rate(10) # 10hz
self.rateSec = rospy.Rate(1) self.rateSec = rospy.Rate(1)
self.hasPos = False self.hasPos = False

8
integrationtests/demo_tests/mavros_tests.launch
View File

@ -3,7 +3,8 @@
<arg name="gui" default="false"/> <arg name="gui" default="false"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="false"/> <arg name="enable_ground_truth" default="false"/>
<arg name="log_file" default="iris"/> <arg name="ns" default="iris"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> <include file="$(find px4)/launch/gazebo_iris_empty_world.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
@ -11,8 +12,11 @@
<arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
<arg name="ns" value="$(arg ns)"/>
</include>
<include file="$(find px4)/launch/mavros_sitl.launch">
<arg name="ns" value="$(arg ns)"/>
</include> </include>
<include file="$(find px4)/launch/mavros_sitl.launch" />
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" /> <test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" />
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> <test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" />

11
launch/ardrone.launch
View File

@ -1,14 +1,19 @@
<launch> <launch>
<arg name="ns"/>
<include file="$(find px4)/launch/multicopter_x.launch" /> <include file="$(find px4)/launch/multicopter_x.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="px4_multicopter"> <group ns="$(arg ns)">
<param name="MPC_XY_P" type="double" value="1.0" /> <param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.0" /> <param name="MPC_XY_FF" type="double" value="0.1" />
<param name="MPC_XY_VEL_P" type="double" value="0.01" /> <param name="MPC_XY_VEL_P" type="double" value="0.01" />
<param name="MPC_XY_VEL_I" type="double" value="0.0" /> <param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" /> <param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> <param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPC_Z_VEL_P" type="double" value="0.3" />
<param name="MPC_Z_P" type="double" value="2" />
<param name="vehicle_model" type="string" value="ardrone" /> <param name="vehicle_model" type="string" value="ardrone" />
</group> </group>

9
launch/gazebo_ardrone_empty_world.launch
View File

@ -4,15 +4,18 @@
<arg name="gui" default="true"/> <arg name="gui" default="true"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="false"/> <arg name="enable_ground_truth" default="false"/>
<arg name="log_file" default="ardrone"/> <arg name="ns" default="ardrone"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch"> <include file="$(find rotors_gazebo)/launch/ardrone_empty_world.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/> <arg name="gui" value="$(arg gui)"/>
<arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
</include> </include>
<include file="$(find px4)/launch/ardrone.launch" /> <include file="$(find px4)/launch/ardrone.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
</launch> </launch>

15
launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
View File

@ -1,9 +1,14 @@
<launch> <launch>
<arg name="ns" default="ardrone"/>
<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> <include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch">
<include file="$(find px4)/launch/mavros_sitl.launch" /> <arg name="ns" value="$(arg ns)"/>
</include>
<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/> <include file="$(find px4)/launch/mavros_sitl.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="$(arg ns)">
<node pkg="px4" name="demo_offboard_attitude_setpoints" type="demo_offboard_attitude_setpoints"/>
</group>
</launch> </launch>

15
launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
View File

@ -1,9 +1,14 @@
<launch> <launch>
<arg name="ns" default="ardrone"/>
<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" /> <include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch">
<include file="$(find px4)/launch/mavros_sitl.launch" /> <arg name="ns" value="$(arg ns)"/>
</include>
<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/> <include file="$(find px4)/launch/mavros_sitl.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="$(arg ns)">
<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_position_setpoints"/>
</group>
</launch> </launch>

9
launch/gazebo_ardrone_house_world.launch
View File

@ -4,15 +4,18 @@
<arg name="gui" default="true"/> <arg name="gui" default="true"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/> <arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="ardrone"/> <arg name="ns" default="ardrone"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find rotors_gazebo)/launch/ardrone_house_world_with_joy.launch"> <include file="$(find rotors_gazebo)/launch/ardrone_house_world.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/> <arg name="gui" value="$(arg gui)"/>
<arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
</include> </include>
<include file="$(find px4)/launch/ardrone.launch" /> <include file="$(find px4)/launch/ardrone.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
</launch> </launch>

9
launch/gazebo_iris_empty_world.launch
View File

@ -4,15 +4,18 @@
<arg name="gui" default="true"/> <arg name="gui" default="true"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/> <arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="iris"/> <arg name="ns" default="iris"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find rotors_gazebo)/launch/iris_empty_world_with_joy.launch"> <include file="$(find rotors_gazebo)/launch/iris_empty_world.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
<arg name="gui" value="$(arg gui)"/> <arg name="gui" value="$(arg gui)"/>
<arg name="enable_logging" value="$(arg enable_logging)" /> <arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
</include> </include>
<include file="$(find px4)/launch/iris.launch" /> <include file="$(find px4)/launch/iris.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
</launch> </launch>

7
launch/gazebo_iris_house_world.launch
View File

@ -4,7 +4,8 @@
<arg name="gui" default="true"/> <arg name="gui" default="true"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/> <arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="iris"/> <arg name="ns" default="iris"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find rotors_gazebo)/launch/iris_house_world_with_joy.launch"> <include file="$(find rotors_gazebo)/launch/iris_house_world_with_joy.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
@ -13,6 +14,8 @@
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
</include> </include>
<include file="$(find px4)/launch/iris.launch" /> <include file="$(find px4)/launch/iris.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
</launch> </launch>

7
launch/gazebo_iris_outdoor_world.launch
View File

@ -4,7 +4,8 @@
<arg name="gui" default="true"/> <arg name="gui" default="true"/>
<arg name="enable_logging" default="false"/> <arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/> <arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="iris"/> <arg name="ns" default="iris"/>
<arg name="log_file" default="$(arg ns)"/>
<include file="$(find rotors_gazebo)/launch/iris_outdoor_world_with_joy.launch"> <include file="$(find rotors_gazebo)/launch/iris_outdoor_world_with_joy.launch">
<arg name="headless" value="$(arg headless)"/> <arg name="headless" value="$(arg headless)"/>
@ -13,6 +14,8 @@
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/> <arg name="log_file" value="$(arg log_file)"/>
</include> </include>
<include file="$(find px4)/launch/iris.launch" /> <include file="$(find px4)/launch/iris.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
</launch> </launch>

9
launch/iris.launch
View File

@ -1,8 +1,11 @@
<launch> <launch>
<arg name="ns"/>
<include file="$(find px4)/launch/multicopter_w.launch" /> <include file="$(find px4)/launch/multicopter_w.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="px4_multicopter"> <group ns="$(arg ns)">
<param name="mixer" type="string" value="i" /> <param name="mixer" type="string" value="i" />
<param name="MPC_XY_P" type="double" value="1.0" /> <param name="MPC_XY_P" type="double" value="1.0" />
<param name="MPC_XY_FF" type="double" value="0.0" /> <param name="MPC_XY_FF" type="double" value="0.0" />
@ -10,6 +13,8 @@
<param name="MPC_XY_VEL_I" type="double" value="0.0" /> <param name="MPC_XY_VEL_I" type="double" value="0.0" />
<param name="MPC_XY_VEL_D" type="double" value="0.01" /> <param name="MPC_XY_VEL_D" type="double" value="0.01" />
<param name="MPC_XY_VEL_MAX" type="double" value="2.0" /> <param name="MPC_XY_VEL_MAX" type="double" value="2.0" />
<param name="MPC_Z_VEL_P" type="double" value="0.3" />
<param name="MPC_Z_P" type="double" value="2" />
<param name="vehicle_model" type="string" value="iris" /> <param name="vehicle_model" type="string" value="iris" />
</group> </group>

7
launch/mavros_sitl.launch
View File

@ -1,7 +1,9 @@
<launch> <launch>
<!-- vim: set ft=xml noet : --> <!-- vim: set ft=xml noet : -->
<!-- example launch script for PX4 based FCU's --> <!-- example launch script for PX4 based FCU's -->
<arg name="ns" default="/" />
<group ns="$(arg ns)">
<arg name="fcu_url" default="udp://localhost:14560@localhost:14565" /> <arg name="fcu_url" default="udp://localhost:14560@localhost:14565" />
<arg name="gcs_url" default="" /> <arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" /> <arg name="tgt_system" default="1" />
@ -18,4 +20,5 @@
<arg name="tgt_system" value="$(arg tgt_system)" /> <arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" /> <arg name="tgt_component" value="$(arg tgt_component)" />
</include> </include>
</group>
</launch> </launch>

3
launch/multicopter.launch
View File

@ -1,6 +1,7 @@
<launch> <launch>
<arg name="ns"/>
<group ns="px4_multicopter"> <group ns="$(arg ns)">
<node pkg="joy" name="joy_node" type="joy_node"/> <node pkg="joy" name="joy_node" type="joy_node"/>
<node pkg="px4" name="manual_input" type="manual_input"/> <node pkg="px4" name="manual_input" type="manual_input"/>
<node pkg="px4" name="commander" type="commander"/> <node pkg="px4" name="commander" type="commander"/>

7
launch/multicopter_w.launch
View File

@ -1,8 +1,11 @@
<launch> <launch>
<arg name="ns"/>
<include file="$(find px4)/launch/multicopter.launch" /> <include file="$(find px4)/launch/multicopter.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="px4_multicopter"> <group ns="$(arg ns)">
<param name="mixer" type="string" value="w" /> <param name="mixer" type="string" value="w" />
</group> </group>

7
launch/multicopter_x.launch
View File

@ -1,8 +1,11 @@
<launch> <launch>
<arg name="ns"/>
<include file="$(find px4)/launch/multicopter.launch" /> <include file="$(find px4)/launch/multicopter.launch">
<arg name="ns" value="$(arg ns)"/>
</include>
<group ns="px4_multicopter"> <group ns="$(arg ns)">
<param name="mixer" type="string" value="x" /> <param name="mixer" type="string" value="x" />
</group> </group>

6
src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
View File

@ -40,7 +40,7 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <px4.h> #include <px4.h>
#include <lib/mathlib/mathlib.h> #include <lib/mathlib/mathlib.h>
#include <mav_msgs/MotorSpeed.h> #include <mav_msgs/CommandMotorSpeed.h>
#include <string> #include <string>
class MultirotorMixer class MultirotorMixer
@ -129,7 +129,7 @@ MultirotorMixer::MultirotorMixer():
_rotors(_config_index[0]) _rotors(_config_index[0])
{ {
_sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
_pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10); _pub = _n.advertise<mav_msgs::CommandMotorSpeed>("command/motor_speed", 10);
if (!_n.hasParam("motor_scaling_radps")) { if (!_n.hasParam("motor_scaling_radps")) {
_n.setParam("motor_scaling_radps", 150.0); _n.setParam("motor_scaling_radps", 150.0);
@ -237,7 +237,7 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m
mix(); mix();
// publish message // publish message
mav_msgs::MotorSpeed rotor_vel_msg; mav_msgs::CommandMotorSpeed rotor_vel_msg;
double scaling; double scaling;
double offset; double offset;
_n.getParamCached("motor_scaling_radps", scaling); _n.getParamCached("motor_scaling_radps", scaling);