mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-20 08:41:36 +08:00
Merge pull request #1988 from UAVenture/mavros_update
MAVROS update for SITL, updated tests and demo nodes
This commit is contained in:
@@ -67,8 +67,8 @@ class ManualInputTest(unittest.TestCase):
|
||||
#
|
||||
def test_manual_input(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
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)
|
||||
rospy.Subscriber('actuator_armed', actuator_armed, self.actuator_armed_callback)
|
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
|
||||
|
||||
man_in = ManualInput()
|
||||
|
||||
|
||||
@@ -68,9 +68,9 @@ class DirectOffboardPosctlTest(unittest.TestCase):
|
||||
self.helper = PX4TestHelper("direct_offboard_posctl_test")
|
||||
self.helper.setUp()
|
||||
|
||||
rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
|
||||
self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback)
|
||||
self.pub_spt = rospy.Publisher('iris/position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
|
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
|
||||
self.sub_vlp = rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback)
|
||||
self.pub_spt = rospy.Publisher('position_setpoint_triplet', position_setpoint_triplet, queue_size=10)
|
||||
self.rate = rospy.Rate(10) # 10hz
|
||||
self.has_pos = False
|
||||
self.local_position = vehicle_local_position()
|
||||
|
||||
@@ -3,7 +3,8 @@
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="enable_logging" default="false"/>
|
||||
<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 px4)/launch/gazebo_iris_empty_world.launch">
|
||||
<arg name="headless" value="$(arg headless)"/>
|
||||
@@ -11,8 +12,11 @@
|
||||
<arg name="enable_logging" value="$(arg enable_logging)" />
|
||||
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
|
||||
<arg name="log_file" value="$(arg log_file)"/>
|
||||
<arg name="ns" value="$(arg ns)"/>
|
||||
</include>
|
||||
|
||||
<test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" />
|
||||
<test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" />
|
||||
<group ns="$(arg ns)">
|
||||
<test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" />
|
||||
<test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" />
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -62,10 +62,10 @@ class FlightPathAssertion(threading.Thread):
|
||||
#
|
||||
def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2):
|
||||
threading.Thread.__init__(self)
|
||||
rospy.Subscriber("iris/vehicle_local_position", vehicle_local_position, self.position_callback)
|
||||
self.spawn_model = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
|
||||
self.set_model_state = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
|
||||
self.delete_model = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
|
||||
rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback)
|
||||
self.spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
|
||||
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
|
||||
self.delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
|
||||
|
||||
self.positions = positions
|
||||
self.tunnel_radius = tunnelRadius
|
||||
|
||||
@@ -49,8 +49,8 @@ class ManualInput(object):
|
||||
|
||||
def __init__(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10)
|
||||
self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10)
|
||||
self.pub_mcsp = rospy.Publisher('manual_control_setpoint', manual_control_setpoint, queue_size=10)
|
||||
self.pub_off = rospy.Publisher('offboard_control_mode', offboard_control_mode, queue_size=10)
|
||||
|
||||
def arm(self):
|
||||
rate = rospy.Rate(10) # 10hz
|
||||
|
||||
@@ -61,14 +61,14 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
|
||||
|
||||
def setUp(self):
|
||||
rospy.init_node('test_node', anonymous=True)
|
||||
rospy.wait_for_service('iris/mavros/cmd/arming', 30)
|
||||
rospy.wait_for_service('mavros/cmd/arming', 30)
|
||||
self.helper = PX4TestHelper("mavros_offboard_attctl_test")
|
||||
self.helper.setUp()
|
||||
|
||||
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.pub_att = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10)
|
||||
self.pub_thr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10)
|
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
|
||||
rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
|
||||
self.pub_att = rospy.Publisher('mavros/setpoint_attitude/attitude', PoseStamped, queue_size=10)
|
||||
self.pub_thr = rospy.Publisher('mavros/setpoint_attitude/att_throttle', Float64, queue_size=10)
|
||||
self.rate = rospy.Rate(10) # 10hz
|
||||
self.has_pos = False
|
||||
self.control_mode = vehicle_control_mode()
|
||||
@@ -110,9 +110,9 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
|
||||
att.header.stamp = rospy.Time.now()
|
||||
|
||||
self.pub_att.publish(att)
|
||||
self.helper.bag_write('mavros/setpoint/attitude', att)
|
||||
self.helper.bag_write('mavros/setpoint_attitude/attitude', att)
|
||||
self.pub_thr.publish(throttle)
|
||||
self.helper.bag_write('mavros/setpoint/att_throttle', throttle)
|
||||
self.helper.bag_write('mavros/setpoint_attitude/att_throttle', throttle)
|
||||
self.rate.sleep()
|
||||
|
||||
if (self.local_position.pose.position.x > 5
|
||||
|
||||
@@ -67,9 +67,9 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
|
||||
self.helper = PX4TestHelper("mavros_offboard_posctl_test")
|
||||
self.helper.setUp()
|
||||
|
||||
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.pub_spt = rospy.Publisher('iris/mavros/setpoint/local_position', PoseStamped, queue_size=10)
|
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback)
|
||||
rospy.Subscriber("mavros/local_position/local", PoseStamped, self.position_callback)
|
||||
self.pub_spt = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
|
||||
self.rate = rospy.Rate(10) # 10hz
|
||||
self.has_pos = False
|
||||
self.local_position = PoseStamped()
|
||||
@@ -128,7 +128,7 @@ class MavrosOffboardPosctlTest(unittest.TestCase):
|
||||
# update timestamp for each published SP
|
||||
pos.header.stamp = rospy.Time.now()
|
||||
self.pub_spt.publish(pos)
|
||||
self.helper.bag_write('mavros/setpoint/local_position', pos)
|
||||
self.helper.bag_write('mavros/setpoint_position/local', pos)
|
||||
|
||||
if self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5):
|
||||
break
|
||||
|
||||
@@ -12,12 +12,14 @@
|
||||
<arg name="enable_logging" value="$(arg enable_logging)" />
|
||||
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
|
||||
<arg name="log_file" value="$(arg log_file)"/>
|
||||
<arg name="ns" value="$(arg ns)"/>
|
||||
<arg name="ns" value="$(arg ns)"/>
|
||||
</include>
|
||||
<include file="$(find px4)/launch/mavros_sitl.launch">
|
||||
<arg name="ns" value="$(arg ns)"/>
|
||||
<arg name="ns" value="$(arg ns)"/>
|
||||
</include>
|
||||
|
||||
<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" />
|
||||
<group ns="$(arg ns)">
|
||||
<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" />
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
@@ -9,10 +9,8 @@
|
||||
<arg name="tgt_system" default="1" />
|
||||
<arg name="tgt_component" default="50" />
|
||||
|
||||
<param name="mavros/setpoint/attitude/listen_twist" type="bool" value="false" />
|
||||
|
||||
<include file="$(find mavros)/launch/node.launch">
|
||||
<arg name="blacklist_yaml" value="$(find mavros)/launch/px4_blacklist.yaml" />
|
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
|
||||
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
|
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
|
||||
+2
-2
@@ -49,8 +49,8 @@
|
||||
|
||||
DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
|
||||
_n(),
|
||||
_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)),
|
||||
_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1))
|
||||
_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_attitude/attitude", 1)),
|
||||
_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint_attitude/att_throttle", 1))
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
+1
-1
@@ -46,7 +46,7 @@
|
||||
|
||||
DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() :
|
||||
_n(),
|
||||
_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1))
|
||||
_local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 1))
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user