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 @@
+
+
+
+
-
diff --git a/launch/ardrone.launch b/launch/ardrone.launch
index 3173e7cf1a..56030dd1f4 100644
--- a/launch/ardrone.launch
+++ b/launch/ardrone.launch
@@ -1,14 +1,19 @@
+
-
+
+
+
-
+
-
+
+
+
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..1309529b63 100644
--- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
+++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch
@@ -1,9 +1,14 @@
-
+
-
-
-
-
+
+
+
+
+
+
+
+
+
diff --git a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
index 9ff7f7d1fa..0371306ce5 100644
--- a/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
+++ b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch
@@ -1,9 +1,14 @@
-
+
-
-
-
-
+
+
+
+
+
+
+
+
+
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..bf5b099b64 100644
--- a/launch/iris.launch
+++ b/launch/iris.launch
@@ -1,8 +1,11 @@
+
-
+
+
+
-
+
@@ -10,6 +13,8 @@
+
+
diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch
index 40010a2735..7362fa4644 100644
--- a/launch/mavros_sitl.launch
+++ b/launch/mavros_sitl.launch
@@ -1,7 +1,9 @@
-
-
+
+
+
+
@@ -18,4 +20,5 @@
+
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);