Merge branch 'noetic-devel' into issue-161

Author: pariaspe

drone_wrapper/launch/basic_config.yml

@@ -15,8 +15,6 @@
     <arg name="model" default="iris_dual_cam"/>
     <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
     <arg name="sdf" default="$(find drone_assets)/models/$(arg model)/$(arg model).sdf"/>
-	<!-- PX4 param config -->
-	<arg name="config" default="$(find drone_wrapper)/launch/basic_config.yml"/>
 
     <!-- gazebo configs -->
     <arg name="gui" default="true"/>
@@ -54,6 +52,4 @@
         <arg name="fcu_url" value="$(arg fcu_url)"/>
         <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
     </include>
-	<!-- PX4 param configuration -->
-	<node name="my_params" pkg="drone_wrapper" type="load_parameters" args="$(arg config)"/>
 </launch>

drone_wrapper/launch/mavros_px4_sitl.launch

@@ -330,21 +330,18 @@ def param_set(self, param, value):
         else:
             val = ParamValue(integer=value, real=0.0)
 
-        rospy.wait_for_service(self.ns + '/mavros/param/set')
         try:
-            set_param = rospy.ServiceProxy(self.ns + '/mavros/param/set', ParamSet)
-            resp = set_param(param_id=param, value=val)
-            print("setmode send ok", resp.success)
+            resp = self.set_param(param_id=param, value=val)
+            rospy.loginfo("ParamSet: %s", resp.success)
         except rospy.ServiceException as e:
-            print("Failed SetMode:", e)
+            rospy.logwarn("Failed ParamSet: %s", e)
 
     def param_get(self, param):
         try:
-            get_param = rospy.ServiceProxy(self.ns + 'mavros/param/get', ParamGet)
-            resp = get_param(param_id=param)
-            print("setmode send ok", resp.success)
+            resp = self.get_param(param_id=param)
+            rospy.loginfo("ParamGet: %s", resp.success)
         except rospy.ServiceException as e:
-            print("Failed SetMode:", e)
+            rospy.logwarn("Failed ParamGet: %s", e)
             return None
 
         if resp.value.integer != 0:
@@ -376,7 +373,7 @@ def request_mode(self, mode='OFFBOARD'):
             rospy.logwarn('Mode change request unsuccessful')
             return False
 
-    def set_cmd_pos(self, x=0, y=0, z=0, az=0, frame="map"):
+    def set_cmd_pos(self, x=0, y=0, z=0, az=0, max_vel=12.0, frame="map"):
         pose = Pose(position=Point(x=x, y=y, z=z),
                     orientation=Quaternion(*tf.transformations.quaternion_from_euler(0, 0, az)))
         pose = self.frames_tf.transform(frame, "map", pose).pose
@@ -404,6 +401,7 @@ def set_cmd_pos(self, x=0, y=0, z=0, az=0, frame="map"):
         CMD = 0  # POS
         self.setpoint_raw.type_mask = 3064  # xyz yaw
 
+        self.param_set(param="MPC_XY_VEL_MAX", value=float(max_vel))
         self.setpoint_raw_publisher.publish(self.setpoint_raw)
 
     def set_cmd_vel(self, vx=0, vy=0, vz=0, az=0, frame="flu"):
@@ -629,6 +627,10 @@ def __init__(self, name='drone', ns='', verbose=False):
         self.mode_client = rospy.ServiceProxy(ns + 'mavros/set_mode', SetMode)
         rospy.wait_for_service(self.ns + 'mavros/cmd/land')
         self.land_client = rospy.ServiceProxy(ns + 'mavros/cmd/land', CommandTOL)
+        rospy.wait_for_service(self.ns + '/mavros/param/set')
+        self.set_param = rospy.ServiceProxy(self.ns + '/mavros/param/set', ParamSet)
+        rospy.wait_for_service(self.ns + '/mavros/param/get')
+        self.get_param = rospy.ServiceProxy(self.ns + 'mavros/param/get', ParamGet)
 
         self.rqt_extended_state_publisher = rospy.Publisher(self.ns + 'drone_wrapper/extended_state', ExtendedState,
                                                             queue_size=1)

drone_wrapper/scripts/load_parameters

@@ -0,0 +1,13 @@
+#!/usr/bin/env python
+
+from drone_wrapper import DroneWrapper
+import time
+
+d = DroneWrapper()
+d.takeoff()
+
+d.set_cmd_pos(0, 0, 3, max_vel=6.0)
+
+while True:
+	print(d.get_velocity())
+	time.sleep(0.2)