From c07c39bc438695101dc58d2fda2e14914e019ca4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 28 Feb 2015 18:15:51 +0100 Subject: [PATCH 1/5] update ros launch files and nodes for update of rotors_simulator --- launch/ardrone.launch | 7 +++++-- launch/gazebo_ardrone_empty_world.launch | 9 ++++++--- ...o_ardrone_empty_world_offboard_attitudedemo.launch | 11 +++++++---- ...o_ardrone_empty_world_offboard_positiondemo.launch | 11 +++++++---- launch/gazebo_ardrone_house_world.launch | 9 ++++++--- launch/gazebo_iris_empty_world.launch | 9 ++++++--- launch/gazebo_iris_house_world.launch | 7 +++++-- launch/gazebo_iris_outdoor_world.launch | 7 +++++-- launch/iris.launch | 7 +++++-- launch/multicopter.launch | 3 ++- launch/multicopter_w.launch | 7 +++++-- launch/multicopter_x.launch | 7 +++++-- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 6 +++--- 13 files changed, 67 insertions(+), 33 deletions(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 3173e7cf1a..809eab2924 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 22bfb0c799..122a27867d 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 717244abf5..596496194a 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -1,9 +1,12 @@ - + - + + + - - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 9ff7f7d1fa..46f427f936 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -1,9 +1,12 @@ - + - + + + - - + + + diff --git a/launch/gazebo_ardrone_house_world.launch b/launch/gazebo_ardrone_house_world.launch index c636868eb2..89f27c901f 100644 --- a/launch/gazebo_ardrone_house_world.launch +++ b/launch/gazebo_ardrone_house_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_empty_world.launch b/launch/gazebo_iris_empty_world.launch index 8a4c128b90..0e80208e67 100644 --- a/launch/gazebo_iris_empty_world.launch +++ b/launch/gazebo_iris_empty_world.launch @@ -4,15 +4,18 @@ - + + - + - + + + diff --git a/launch/gazebo_iris_house_world.launch b/launch/gazebo_iris_house_world.launch index e4f9f6532c..d0e824d4b5 100644 --- a/launch/gazebo_iris_house_world.launch +++ b/launch/gazebo_iris_house_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/gazebo_iris_outdoor_world.launch b/launch/gazebo_iris_outdoor_world.launch index 22d55502d3..e75e57b877 100644 --- a/launch/gazebo_iris_outdoor_world.launch +++ b/launch/gazebo_iris_outdoor_world.launch @@ -4,7 +4,8 @@ - + + @@ -13,6 +14,8 @@ - + + + diff --git a/launch/iris.launch b/launch/iris.launch index be33cb85f4..fb9c0e6976 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/multicopter.launch b/launch/multicopter.launch index bc0e377715..6882f24137 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -1,6 +1,7 @@ + - + diff --git a/launch/multicopter_w.launch b/launch/multicopter_w.launch index f124082aa9..66c30d186f 100644 --- a/launch/multicopter_w.launch +++ b/launch/multicopter_w.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/launch/multicopter_x.launch b/launch/multicopter_x.launch index 6355b87be9..c686eba390 100644 --- a/launch/multicopter_x.launch +++ b/launch/multicopter_x.launch @@ -1,8 +1,11 @@ + - + + + - + diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 1098ec73b0..9b48294b64 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include class MultirotorMixer @@ -129,7 +129,7 @@ MultirotorMixer::MultirotorMixer(): _rotors(_config_index[0]) { _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this); - _pub = _n.advertise("/mixed_motor_commands", 10); + _pub = _n.advertise("command/motor_speed", 10); if (!_n.hasParam("motor_scaling_radps")) { _n.setParam("motor_scaling_radps", 150.0); @@ -237,7 +237,7 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m mix(); // publish message - mav_msgs::MotorSpeed rotor_vel_msg; + mav_msgs::CommandMotorSpeed rotor_vel_msg; double scaling; double offset; _n.getParamCached("motor_scaling_radps", scaling); From df4f8259026bb7882659303558f5fb5b0d99e5f6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 4 Mar 2015 20:58:54 +0100 Subject: [PATCH 2/5] ros offboard demo: move mavros into model namespace --- ...gazebo_ardrone_empty_world_offboard_attitudedemo.launch | 4 +++- ...gazebo_ardrone_empty_world_offboard_positiondemo.launch | 4 +++- launch/mavros_sitl.launch | 7 +++++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 596496194a..1309529b63 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -4,7 +4,9 @@ - + + + diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch index 46f427f936..0371306ce5 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch @@ -4,7 +4,9 @@ - + + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 40010a2735..7752c682d1 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -1,7 +1,9 @@ - - + + + + @@ -18,4 +20,5 @@ + From 9a9ca184e80622f2e217b4397be78a74cb1dad7f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 7 Mar 2015 19:27:36 +0100 Subject: [PATCH 3/5] update namespace in mavros tests --- .../demo_tests/direct_manual_input_test.py | 4 ++-- .../demo_tests/direct_offboard_posctl_test.py | 6 +++--- integrationtests/demo_tests/manual_input.py | 6 +++--- .../demo_tests/mavros_offboard_attctl_test.py | 18 +++++++++--------- .../demo_tests/mavros_offboard_posctl_test.py | 18 +++++++++--------- .../demo_tests/mavros_tests.launch | 8 ++++++-- 6 files changed, 32 insertions(+), 28 deletions(-) diff --git a/integrationtests/demo_tests/direct_manual_input_test.py b/integrationtests/demo_tests/direct_manual_input_test.py index 6d115316b2..b037d842f8 100755 --- a/integrationtests/demo_tests/direct_manual_input_test.py +++ b/integrationtests/demo_tests/direct_manual_input_test.py @@ -64,8 +64,8 @@ class ManualInputTest(unittest.TestCase): # def test_manual_input(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/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/actuator_armed', actuator_armed, self.actuator_armed_callback) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) man = ManualInput() diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index e09550bbc2..18239e9264 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -66,9 +66,9 @@ class DirectOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz def tearDown(self): diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 9b2471a00e..7072464dec 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -54,8 +54,8 @@ class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/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.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, 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) def arm(self): @@ -85,7 +85,7 @@ class ManualInput: # Fake input to iris commander self.pubAtt.publish(att) rate.sleep() - count = count + 1 + count = count + 1 pos.r = 1 count = 0 diff --git a/integrationtests/demo_tests/mavros_offboard_attctl_test.py b/integrationtests/demo_tests/mavros_offboard_attctl_test.py index a52f7ffc1b..92cdf1e0ad 100755 --- a/integrationtests/demo_tests/mavros_offboard_attctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_attctl_test.py @@ -65,12 +65,12 @@ class MavrosOffboardAttctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) - self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + rospy.wait_for_service('iris/mavros/cmd/arming', 30) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) + self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) + self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.rateSec = rospy.Rate(1) self.hasPos = False @@ -109,7 +109,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): # set some attitude and thrust att = PoseStamped() - att.header = Header() + att.header = Header() att.header.frame_id = "base_footprint" att.header.stamp = rospy.Time.now() quaternion = quaternion_from_euler(0.15, 0.15, 0) @@ -126,7 +126,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): att.header.stamp = rospy.Time.now() self.pubAtt.publish(att) self.pubThr.publish(throttle) - + if (self.localPosition.pose.position.x > 5 and self.localPosition.pose.position.z > 5 and self.localPosition.pose.position.y < -5): @@ -135,7 +135,7 @@ class MavrosOffboardAttctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count < timeout, "took too long to cross boundaries") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py index a3739ae5ce..6d26015e7b 100755 --- a/integrationtests/demo_tests/mavros_offboard_posctl_test.py +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -64,11 +64,11 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def setUp(self): rospy.init_node('test_node', anonymous=True) - rospy.wait_for_service('mavros/cmd/arming', 30) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) - self.pubSpt = rospy.Publisher('mavros/setpoint/local_position', PoseStamped, queue_size=10) - self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) + rospy.wait_for_service('iris/mavros/cmd/arming', 30) + rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) self.rate = rospy.Rate(10) # 10hz self.rateSec = rospy.Rate(1) self.hasPos = False @@ -100,7 +100,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): def reach_position(self, x, y, z, timeout): # set a position setpoint pos = PoseStamped() - pos.header = Header() + pos.header = Header() pos.header.frame_id = "base_footprint" pos.pose.position.x = x pos.pose.position.y = y @@ -118,7 +118,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): # update timestamp for each published SP pos.header.stamp = rospy.Time.now() self.pubSpt.publish(pos) - + if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): break count = count + 1 @@ -153,7 +153,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): for i in range(0, len(positions)): self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - + # does it hold the position for Y seconds? positionHeld = True count = 0 @@ -166,7 +166,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") - + if __name__ == '__main__': import rostest diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index 4651f0dc9c..e42db60434 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -3,7 +3,8 @@ - + + @@ -11,8 +12,11 @@ + + + + - From 2883edaecd442e3049ad3224989cb384096e637a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 7 Mar 2015 23:38:48 +0100 Subject: [PATCH 4/5] ros sitl: increase Z gains for ardrone and iris --- launch/ardrone.launch | 4 +++- launch/iris.launch | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/ardrone.launch b/launch/ardrone.launch index 809eab2924..56030dd1f4 100644 --- a/launch/ardrone.launch +++ b/launch/ardrone.launch @@ -7,11 +7,13 @@ - + + + diff --git a/launch/iris.launch b/launch/iris.launch index fb9c0e6976..bf5b099b64 100644 --- a/launch/iris.launch +++ b/launch/iris.launch @@ -13,6 +13,8 @@ + + From b311f7302b508d1ed7fd3cfecd0bfbe47b324bde Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Mar 2015 21:24:07 +0100 Subject: [PATCH 5/5] mavros sitl launch file: add default namespace --- launch/mavros_sitl.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 7752c682d1..7362fa4644 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -2,7 +2,7 @@ - +