forked from xtdrone/XTDrone
一些修改
This commit is contained in:
parent
ea60f258d7
commit
4ae05f2422
|
@ -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:
|
||||
|
|
|
@ -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
|
||||
k : hover(hover mode) 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 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':
|
||||
cmd = 'HOVER'
|
||||
cmd_vel_mask = False
|
||||
print_msg()
|
||||
print('Hover')
|
||||
elif key == 's' :
|
||||
elif key == 'k' or key == 's':
|
||||
cmd_vel_mask = False
|
||||
forward = 0.0
|
||||
leftward = 0.0
|
|
@ -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>
|
|
@ -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) -->
|
|
@ -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>
|
|
@ -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="sdf" default="$(find mavlink_sitl_gazebo)/models/rover_with_sensors/rover_with_sensors.sdf"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/outdoor3.world"/>
|
||||
<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"/>
|
||||
|
|
|
@ -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>
|
Loading…
Reference in New Issue