一些修改

communication/multirotor_communication.py

@@ -141,28 +141,36 @@ class Communication:
         self.target_motion = self.construct_target(x=msg.position.x,y=msg.position.y,z=msg.position.z)
         
     def cmd_vel_flu_callback(self, msg):
+        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
         if self.hover_flag == 0:
             self.coordinate_frame = 8
             self.motion_type = 1     
             self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)       
  
     def cmd_vel_enu_callback(self, msg):
+        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
         if self.hover_flag == 0:
             self.coordinate_frame = 1
             self.motion_type = 1
             self.target_motion = self.construct_target(vx=msg.linear.x,vy=msg.linear.y,vz=msg.linear.z,yaw_rate=msg.angular.z)
 
     def cmd_accel_flu_callback(self, msg):
+        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
         if self.hover_flag == 0:
             self.coordinate_frame = 8
             self.motion_type = 2
             self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
             
     def cmd_accel_enu_callback(self, msg):
+        self.hover_state_transition(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.z)
         if self.hover_flag == 0:
             self.coordinate_frame = 1 
             self.motion_type = 2
-            self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.x,afz=msg.linear.x,yaw_rate=msg.angular.z)
+            self.target_motion = self.construct_target(afx=msg.linear.x,afy=msg.linear.y,afz=msg.linear.z,yaw_rate=msg.angular.z)
+            
+    def hover_state_transition(self,x,y,z,w):
+        if abs(x) > 0.005 or abs(y)  > 0.005 or abs(z)  > 0.005 or abs(w)  > 0.005:
+            self.hover_flag = 0
 
     def cmd_callback(self, msg):
         if msg.data == '':
@@ -218,7 +226,6 @@ class Communication:
             self.hover()
             print(self.vehicle_type+'_'+self.vehicle_id+":"+self.flight_mode)
         elif self.flightModeService(custom_mode=self.flight_mode):
-            self.hover_flag = 0
             print(self.vehicle_type+'_'+self.vehicle_id+": "+self.flight_mode)
             return True
         else:

control/multirotor_keyboard_control.py

@@ -30,8 +30,7 @@ r   : return home
 t/y : arm/disarm
 v/n : takeoff/land
 b   : offboard
-s   : hover(offboard mode) and remove the mask of keyboard control
+s/k : hover and remove the mask of keyboard control
-k   : hover(hover mode) and remove the mask of keyboard control
 0~9 : extendable mission(eg.different formation configuration)
       this will mask the keyboard control
 g   : control the leader
@@ -55,7 +54,7 @@ r   : return home
 t/y : arm/disarm
 v/n : takeoff(disenabled now)/land
 b   : offboard
-s or k : hover and remove the mask of keyboard control
+s/k : hover and remove the mask of keyboard control
 0~9 : extendable mission(eg.different formation configuration)
       this will mask the keyboard control
 g   : control all drones
@@ -208,12 +207,7 @@ if __name__=="__main__":
         elif key == 'g':
             ctrl_leader = not ctrl_leader
             print_msg()
-        elif key == 'k':
+        elif key == 'k' or key == 's':
-            cmd = 'HOVER'
-            cmd_vel_mask = False
-            print_msg()
-            print('Hover')
-        elif key == 's' :
             cmd_vel_mask = False
             forward   = 0.0
             leftward   = 0.0

sitl_config/launch/2d_motion_planning.launch

@@ -1,23 +0,0 @@
-<launch>    
-  <!-- Arguments -->
-  <arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
-  <arg name="move_forward_only" default="false"/>
-  <arg name="open_rviz" default="true"/>
-
-  <!-- Map server -->
-  <node pkg="map_server" name="map_server" type="map_server" args="$(arg motion_planning)/map/indoor3.yaml"/>
-
-  <!-- move_base -->
-  <include file="$(find px4)/launch/move_base.launch">
-    <arg name="motion_planning" value="$(arg motion_planning)"/>
-    <arg name="move_forward_only" value="$(arg move_forward_only)"/>
-    <arg name="cmd_vel_topic" default="/xtdrone/cmd_vel_flu" />
-    <arg name="odom_topic" default="/mavros/local_position/odom" />
-  </include>
-
-  <!-- rviz -->
-  <group if="$(arg open_rviz)"> 
-    <node pkg="rviz" type="rviz" name="rviz" required="true"
-          args="-d $(arg motion_planning)/rviz/2d_motion_planning.rviz"/>
-  </group>
-</launch>

sitl_config/launch/Drone_with_arm.launch

@@ -1,53 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-    <!-- MAVROS posix SITL environment launch script -->
-    <!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
-    <!-- vehicle model and world -->
-    <arg name="est" default="ekf2"/>
-    <arg name="vehicle" default="iris"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
-    <!-- gazebo configs -->
-    <arg name="gui" default="true"/>
-    <arg name="debug" default="false"/>
-    <arg name="verbose" default="false"/>
-    <arg name="paused" default="false"/>
-    <!-- Gazebo sim -->
-    <include file="$(find gazebo_ros)/launch/empty_world.launch">
-        <arg name="gui" value="$(arg gui)"/>
-        <arg name="world_name" value="$(arg world)"/>
-        <arg name="debug" value="$(arg debug)"/>
-        <arg name="verbose" value="$(arg verbose)"/>
-        <arg name="paused" value="$(arg paused)"/>
-    </include>
-    <!-- UAV0 -->
-    <group ns="uav0">
-        <!-- MAVROS and vehicle configs -->
-        <arg name="ID" value="0"/>
-        <arg name="fcu_url" default="udp://:14540@localhost:14580"/>
-        <!-- PX4 SITL and vehicle spawn -->
-        <include file="$(find px4)/launch/single_vehicle_spawn.launch">
-            <arg name="x" value="0"/>
-            <arg name="y" value="0"/>
-            <arg name="z" value="0"/>
-            <arg name="R" value="0"/>
-            <arg name="P" value="0"/>
-            <arg name="Y" value="0"/>
-            <arg name="vehicle" value="$(arg vehicle)_arm"/>
-            <arg name="mavlink_udp_port" value="14560"/>
-            <arg name="mavlink_tcp_port" value="4560"/>
-            <arg name="ID" value="$(arg ID)"/>
-        </include>
-        <!-- MAVROS -->
-        <include file="$(find mavros)/launch/px4.launch">
-            <arg name="fcu_url" value="$(arg fcu_url)"/>
-            <arg name="gcs_url" value=""/>
-            <arg name="tgt_system" value="$(eval 0 + arg('ID'))"/>
-            <arg name="tgt_component" value="1"/>
-        </include>
-    </group>
-</launch>
-<!-- to add more UAVs (up to 10):
-Increase the id
-Change the name space
-Set the FCU to default="udp://:14540+id@localhost:14550+id"
-Set the malink_udp_port to 14560+id) -->

sitl_config/launch/move_base.launch

@@ -1,23 +0,0 @@
-<launch>
-  <!-- Arguments -->
-  <arg name="motion_planning" default="/home/robin/px4/xtdrone/motion_planning"/>
-  <arg name="cmd_vel_topic" default="/cmd_vel" />
-  <arg name="odom_topic" default="odom" />
-  <arg name="move_forward_only" default="false"/>
-
-  <!-- move_base -->
-  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
-    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
-    <rosparam file="$(arg motion_planning)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
-    <rosparam file="$(arg motion_planning)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
-    <rosparam file="$(arg motion_planning)/param/local_costmap_params.yaml" command="load" />
-    <rosparam file="$(arg motion_planning)/param/global_costmap_params.yaml" command="load" />
-    <rosparam file="$(arg motion_planning)/param/move_base_params.yaml" command="load" />
-    <rosparam file="$(arg motion_planning)/param/dwa_local_planner_params.yaml" command="load" />
-    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
-    <remap from="odom" to="$(arg odom_topic)"/>
-    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
-    <param name="DWAPlannerROS/max_vel_y" value="0.0" if="$(arg move_forward_only)" />
-    <param name="DWAPlannerROS/min_vel_y" value="0.0" if="$(arg move_forward_only)" />
-  </node>
-</launch>

sitl_config/launch/outdoor3.launch

@@ -12,8 +12,8 @@
     <!-- vehicle model and world -->
     <arg name="est" default="ekf2"/>
     <arg name="vehicle" default="rover"/>
-    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/sonoma_raceway.world"/>
+    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor3.world"/>
-    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/rover_with_sensors/rover_with_sensors.sdf"/>
+    <arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/rover_with_lidar_stereo/rover_with_lidar_stereo.sdf"/>
 
     <!-- gazebo configs -->
     <arg name="gui" default="true"/>

sitl_config/worlds/outdoor3.world

@@ -0,0 +1,40 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <include>
+      <uri>model://sonoma_raceway</uri>
+      <pose>-295 150 -7 0 0 0</pose>
+    </include>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>38.161479</latitude_deg>
+      <longitude_deg>-122.454630</longitude_deg>
+      <elevation>488.0</elevation>
+    </spherical_coordinates>
+    <physics name='default_physics' default='0' type='ode'>
+      <gravity>0 0 -9.8066</gravity>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>10</iters>
+          <sor>1.3</sor>
+          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>100</contact_max_correcting_vel>
+          <contact_surface_layer>0.001</contact_surface_layer>
+        </constraints>
+      </ode>
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>250</real_time_update_rate>
+      <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
+    </physics>
+  </world>
+</sdf>