p81523470
/
ppovb5fc7
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

submit the hector case--Flight_control

This commit is contained in:
XunMeng2017 2019-03-06 14:06:43 +08:00
parent 832c13ed84
commit a7ec19cd48
73 changed files with 12375 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Flight_control/src/CMakeLists.txt Symbolic link
View File

@ -0,0 +1 @@
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

47
Flight_control/src/enemy_suv/enemy_suv_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,47 @@
################################################################################
# CMake
################################################################################
cmake_minimum_required(VERSION 2.8.3)
project(enemy_suv_description)
################################################################################
# Packages
################################################################################
find_package(catkin REQUIRED COMPONENTS
urdf
xacro
)
################################################################################
# Declare ROS messages, services and actions
################################################################################
################################################################################
# Declare ROS dynamic reconfigure parameters
################################################################################
################################################################################
# Catkin specific configuration
################################################################################
catkin_package(
CATKIN_DEPENDS urdf xacro
)
################################################################################
# Build
################################################################################
include_directories(
${catkin_INCLUDE_DIRS}
)
################################################################################
# Install
################################################################################
install(DIRECTORY meshes materials urdf launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
################################################################################
# Test
################################################################################

41
Flight_control/src/enemy_suv/enemy_suv_description/launch/spawn_enemy_suv.launch Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<launch>
<arg name="suv_name" default="default_enemy"/>
<arg name="model" default="$(find rosplane_sim)/xacro/$(arg suv_name)/$(arg suv_name).xacro"/>
<arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
<arg name="x" default="0.0"/>
<arg name="y" default="0.0"/>
<arg name="yaw" default="0.0"/>
<arg name="z" default="0.1"/>
<arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="$(arg suv_name)"/>
<arg name="enable_wind" default="true"/>
<arg name="robot_namespace" default="/"/>
<arg name="gazebo_namespace" default=""/>
<!-- send the robot XML to param server -->
<param name="robot_description" command="
$(find xacro)/xacro.py '$(arg model)'
suv_name:=$(arg suv_name)
gazebo_namespace:=$(arg gazebo_namespace)/gazebo
robot_namespace:=$(arg robot_namespace)"
/>
<param name="tf_prefix" type="string" value="$(arg tf_prefix)" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_$(arg suv_name)" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description
-urdf
-x $(arg x)
-y $(arg y)
-z $(arg z)
-Y $(arg yaw)
-model $(arg suv_name)"
respawn="false" output="screen">
</node>
</launch>
<!-- -gazebo_namespace $(arg gazebo_namespace)/gazebo -->
<!-- -robot_namespace $(arg robot_namespace) -->

40
Flight_control/src/enemy_suv/enemy_suv_description/launch/xacro_gazebo_display_enemy_suv.launch Normal file
View File

@ -0,0 +1,40 @@
<launch>
<arg name="suv_name" default="enemy"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="true"/>
<arg name="gui" value="true"/>
<arg name="verbose" value="true"/>
<arg name="debug" value="false"/>
<!-- <arg name="world_name" value="$(find enemy_suv_description)/worlds/kunming_airport.world"/> -->
<arg name="world_name" value="$(find enemy_suv_description)/worlds/kunming/kunming.world"/>
</include>
<!-- <group ns="$(arg suv_name)"> -->
<arg name="model" value="$(find enemy_suv_description)/urdf/enemy_suv.urdf.xacro"/>
<include file="$(find enemy_suv_description)/launch/spawn_enemy_suv.launch">
<arg name="suv_name" value="$(arg suv_name)" />
<arg name="tf_prefix" value="$(arg suv_name)" />
<arg name="model" value="$(arg model)" />
<arg name="x" value="0.0" />
<arg name="y" value="0.0" />
<arg name="z" value="191.0" />
</include>
<!-- <node name="enemy_suv_teleop" pkg="enemy_suv_teleop" type="enemy_suv_teleop"/> -->
<!-- </group> -->
<node name="autopilot_pseudo" pkg="suv_move" type="suv_pseudo_controller" output="screen">
<!-- <remap from="state" to="truth"/> -->
</node>
<node name="pathfollower" pkg="suv_move" type="suv_path_follower" output="screen">
<!-- <remap from="state" to="truth"/> -->
</node>
<node name="pathmanager" pkg="suv_move" type="suv_path_manager" output="screen">
<!-- <remap from="state" to="truth"/> -->
</node>
<node name="pathplanner" pkg="suv_move" type="suv_path_planner"/>
</launch>

BIN
Flight_control/src/enemy_suv/enemy_suv_description/materials/textures/suv.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 736 KiB

BIN
Flight_control/src/enemy_suv/enemy_suv_description/materials/textures/wheels_01.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 859 KiB

30
Flight_control/src/enemy_suv/enemy_suv_description/meshes/suv.mtl Normal file
View File

@ -0,0 +1,30 @@
# 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware
# File Created: 02.10.2016 09:23:07
newmtl SUV_Body
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.5882 0.5882 0.5882
Kd 0.5882 0.5882 0.5882
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka suv.png
map_Kd suv.png
newmtl SUV_Wheels
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.5882 0.5882 0.5882
Kd 0.5882 0.5882 0.5882
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka wheels_01.png
map_Kd wheels_01.png

4971
Flight_control/src/enemy_suv/enemy_suv_description/meshes/suv.obj Normal file
View File

File diff suppressed because it is too large Load Diff

14
Flight_control/src/enemy_suv/enemy_suv_description/package.xml Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<package format="2">
<name>enemy_suv_description</name>
<version>0.2.1</version>
<description>
3D models of the enemy_suv for simulation and visualization
</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>Apache 2.0</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>urdf</depend>
<depend>xacro</depend>
<depend>launch</depend>
</package>

138
Flight_control/src/enemy_suv/enemy_suv_description/urdf/enemy_suv.gazebo.xacro Normal file
View File

@ -0,0 +1,138 @@
<?xml version="1.0"?>
<robot name="enemy_suv"
xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:arg name="laser_visual" default="false"/>
<xacro:arg name="imu_visual" default="false"/>
<gazebo>
<plugin name="enemy_suv_controller" filename="libgazebo_ros_diff_drive.so">
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
<legacyMode>false</legacyMode>
<updateRate>100</updateRate>
<leftJoint>wheel_left_joint</leftJoint>
<rightJoint>wheel_right_joint</rightJoint>
<wheelSeparation>2.160</wheelSeparation>
<wheelDiameter>0.66</wheelDiameter>
<wheelAcceleration>10</wheelAcceleration>
<wheelTorque>100</wheelTorque>
</plugin>
</gazebo>
<gazebo>
<plugin name="enemy_suv_groundtruth_sim" filename="libgazebo_ros_p3d.so">
<updateRate>100.0</updateRate>
<bodyName>base_link</bodyName>
<topicName>ground_truth/state</topicName>
<gaussianNoise>0.0</gaussianNoise>
<frameName>world</frameName>
</plugin>
</gazebo>
<gazebo reference="wheel_left_link">
<mu1>100</mu1>
<mu2>100</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/DarkGrey</material>
</gazebo>
<gazebo reference="wheel_right_link">
<mu1>100</mu1>
<mu2>100</mu2>
<kp>500000.0</kp>
<kd>10.0</kd>
<minDepth>0.001</minDepth>
<maxVel>0.1</maxVel>
<fdir1>1 0 0</fdir1>
<material>Gazebo/DarkGrey</material>
</gazebo>
<!-- <gazebo reference="caster_back_link">
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<bodyName>imu_link</bodyName>
<topicName>imu</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>100.0</updateRate>
<imu>
<noise>
<type>gaussian</type>
<rate>
<mean>0.0</mean>
<stddev>2e-4</stddev>
<bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev>
</rate>
<accel>
<mean>0.0</mean>
<stddev>1.7e-2</stddev>
<bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev>
</accel>
</noise>
</imu>
</plugin>
</gazebo>
<gazebo reference="imu_link">
<sensor type="imu" name="imu">
<always_on>true</always_on>
<update_rate>50</update_rate>
<visualize>$(arg imu_visual)</visualize>
</sensor>
<material>Gazebo/Grey</material>
</gazebo>
<gazebo reference="base_scan">
<material>Gazebo/DarkGrey</material>
<sensor type="ray" name="lds_lfcd_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>$(arg laser_visual)</visualize>
<update_rate>1800</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<resolution>1</resolution>
<min_angle>0.0</min_angle>
<max_angle>6.28</max_angle>
</horizontal>
</scan>
<range>
<min>0.120</min>
<max>3.5</max>
<resolution>0.015</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>base_scan</frameName>
</plugin>
</sensor>
</gazebo> -->
</robot>

229
Flight_control/src/enemy_suv/enemy_suv_description/urdf/enemy_suv.urdf.xacro Normal file
View File

@ -0,0 +1,229 @@
<?xml version="1.0" ?>
<robot name="enemy_suv" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="M_PI" value="3.1415926535897931" />
<xacro:include filename="$(find enemy_suv_description)/urdf/enemy_suv.gazebo.xacro"/>
<!-- <link name="base_footprint"/>
<joint name="base_joint" type="fixed">
<parent link="base_footprint"/>
<child link="base_link"/>
<origin xyz="0.0 0.0 0.010" rpy="0 0 0"/>
</joint> -->
<link name="base_link">
<visual>
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
<geometry>
<mesh filename="package://enemy_suv_description/meshes/suv.obj" scale="0.06 0.06 0.06"/>
</geometry>
<!-- <material name="light_black"/> -->
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 -1.57079632679"/>
<geometry>
<mesh filename="package://enemy_suv_description/meshes/suv.obj" scale="0.06 0.06 0.06"/>
</geometry>
</collision>
<inertial>
<origin xyz="0.0 0 0" rpy="0 0 -1.57079632679"/>
<mass value="1"/>
<inertia ixx="0.01" ixy="0.0" ixz="0.0"
iyy="0.01" iyz="0.0"
izz="0.01" />
</inertial>
</link>
<joint name="wheel_left_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_left_link"/>
<origin xyz="2.0 1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_left_link">
<!-- <visual>
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="dark"/>
</visual> -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.18" radius="0.33"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="wheel_right_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_right_link"/>
<origin xyz="2.0 -1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_right_link">
<!-- <visual>
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="dark"/>
</visual> -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.18" radius="0.33"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="wheel_left_back_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_left_back_link"/>
<origin xyz="-2.0 1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_left_back_link">
<!-- <visual>
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="dark"/>
</visual> -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.18" radius="0.33"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="wheel_right_back_joint" type="continuous">
<parent link="base_link"/>
<child link="wheel_right_back_link"/>
<origin xyz="-2.0 -1.08 0.32" rpy="${-M_PI*0.5} 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<link name="wheel_right_back_link">
<!-- <visual>
<origin xyz="0 0 0" rpy="${M_PI*0.5} 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="dark"/>
</visual> -->
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.18" radius="0.33"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<!-- <joint name="caster_back_joint" type="fixed">
<parent link="base_link"/>
<child link="caster_back_link"/>
<origin xyz="-0.081 0 -0.004" rpy="${-M_PI*0.5} 0 0"/>
</joint>
<link name="caster_back_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.030 0.010 0.020"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" />
<mass value="0.01" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="base_link"/>
<child link="imu_link"/>
<origin xyz="-0.032 0 0.068" rpy="0 0 0"/>
</joint>
<link name="imu_link"/>
<joint name="scan_joint" type="fixed">
<parent link="base_link"/>
<child link="base_scan"/>
<origin xyz="-0.032 0 0.171" rpy="0 0 0"/>
</joint>
<link name="base_scan">
<visual>
<origin xyz="0 0 0.0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="dark"/>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<cylinder length="0.0408" radius="0.070"/>
</geometry>
</collision>
<inertial>
<mass value="0.01" />
<origin xyz="0 0 0" />
<inertia ixx="0.001" ixy="0.0" ixz="0.0"
iyy="0.001" iyz="0.0"
izz="0.001" />
</inertial>
</link> -->
</robot>

71
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming.world Normal file
View File

@ -0,0 +1,71 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://kunming</uri>
<pose>0 0 0 0 0 0</pose>
<scene>
<ambient>0.68 0.68 0.68 1.0</ambient>
<sky>
<sunrise/>
<clouds>
<speed>0</speed>
</clouds>
</sky>
</scene>
<!-- <actor name="animated_quadrotor_0">
<include>
<uri>model://quadrotor</uri>
<pose>-1 -2 10 0 0 0</pose>
</include>
<script>
<loop>true</loop>
<auto_start>true</auto_start>
<trajectory id="0" type="square">
<waypoint>
<time>0.0</time>
<pose>-1 -2 10 0 0 0</pose>
</waypoint>
<waypoint>
<time>1.0</time>
<pose>-1 2 10 0 0 0</pose>
</waypoint>
<waypoint>
<time>2.0</time>
<pose>-1 -2 10 0 0 0</pose>
</waypoint>
</trajectory>
</script>
</actor> 不支持gazebo7版本-->
<physics type="ode">
<real_time_update_rate>1000</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<!-- <ode>
<solver>
<type>quick</type>
<iters>20</iters>
<precon_iters>0</precon_iters>
<sor>1.300000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode> -->
</physics>
</world>
</sdf>

BIN
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming/km.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 14 MiB

20
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming/kunming.material Normal file
View File

@ -0,0 +1,20 @@
material kunming/Image
{
technique
{
pass
{
ambient .8 .8 .8 1.000000
diffuse .5 .5 .5 1.000000
specular 1 1 1 5.00000
emissive 0.01 0.01 0.01 1.000000
texture_unit
{
texture km.jpg
filtering trilinear
scale 1 1
}
}
}
}

84
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming/kunming.world Normal file
View File

@ -0,0 +1,84 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- <include>
<uri>model://kunming_airport</uri>
<pose>65 45 250 0 0 0</pose>
</include> -->
<scene>
<ambient>0.68 0.68 0.68 1.0</ambient>
<sky>
<sunrise/>
<clouds>
<speed>0</speed>
</clouds>
</sky>
</scene>
<model name="heightmap">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<heightmap>
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunmingjichang.dem</uri>
<!-- <uri>file://kunmingjichang.dem</uri> -->
<!--<size>150 150 50</size>-->
<!-- <size>1000 1000 1000</size> -->
<pos>0 0 -1900</pos>
</heightmap>
</geometry>
</collision>
<visual name="visual_abcedf">
<geometry>
<heightmap>
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunmingjichang.dem</uri>
<!-- <uri>file://kunmingjichang.dem</uri> -->
<!--<size>150 150 50</size>-->
<!-- <size>1000 1000 1000</size> -->
<pos>0 0 -1900</pos>
</heightmap>
</geometry>
<material>
<script>
<uri>file:///home/zhangshuai/micros_back/src/micros/tools/simulator/turtlebot_simulator/turtlebot_gazebo/worlds/kunming.material</uri>
<!-- <uri>file://kunming.material</uri> -->
<name>kunming/Image</name>
</script>
</material>
</visual>
</link>
</model>
<!-- <physics type="ode">
<real_time_update_rate>50.0</real_time_update_rate>
<max_step_size>0.02</max_step_size>
<real_time_factor>1.0</real_time_factor> -->
<!-- <ode>
<solver>
<type>quick</type>
<iters>20</iters>
<precon_iters>0</precon_iters>
<sor>1.300000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode> -->
<!-- </physics> -->
</world>
</sdf>

1
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming/kunmingjichang.dem Normal file
View File

File diff suppressed because one or more lines are too long

78
Flight_control/src/enemy_suv/enemy_suv_description/worlds/kunming_airport.world Normal file
View File

@ -0,0 +1,78 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://kunming_airport</uri>
<pose>0 0 0 0 0 0</pose>
</include>
<include>
<uri>model://suv</uri> <!-- SUV模型 -->
<pose>602.050000 -729.260000 0 0 0 0.9</pose> <!-- 初始位姿（x,y,z,滚转，俯仰，偏航） -->
<!-- <pose>745.208000 -553.204000 0 0 0 -1.57</pose> -->
</include>
<scene>
<ambient>0.68 0.68 0.68 1.0</ambient>
<sky>
<sunrise/>
<clouds>
<speed>0</speed>
</clouds>
</sky>
</scene>
<!-- <actor name="animated_quadrotor_0">
<include>
<uri>model://quadrotor</uri>
<pose>-1 -2 10 0 0 0</pose>
</include>
<script>
<loop>true</loop>
<auto_start>true</auto_start>
<trajectory id="0" type="square">
<waypoint>
<time>0.0</time>
<pose>-1 -2 10 0 0 0</pose>
</waypoint>
<waypoint>
<time>1.0</time>
<pose>-1 2 10 0 0 0</pose>
</waypoint>
<waypoint>
<time>2.0</time>
<pose>-1 -2 10 0 0 0</pose>
</waypoint>
</trajectory>
</script>
</actor> 不支持gazebo7版本-->
<physics type="ode">
<real_time_update_rate>1000</real_time_update_rate>
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
<!-- <ode>
<solver>
<type>quick</type>
<iters>20</iters>
<precon_iters>0</precon_iters>
<sor>1.300000</sor>
<use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0.00</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>10000.000000</contact_max_correcting_vel>
<contact_surface_layer>0.01000</contact_surface_layer>
</constraints>
</ode> -->
</physics>
</world>
</sdf>

29
Flight_control/src/enemy_suv/enemy_suv_teleop/CHANGELOG.rst Normal file
View File

@ -0,0 +1,29 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_teleop
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
* Add optional parameter to set the joystick device
* Contributors: whoenig
0.3.3 (2014-09-01)
------------------
* added run dependency to joy package (joystick drivers)
* Contributors: Johannes Meyer
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-12-26)
------------------
* added slow button (btn 6 on Logitech Gamepad)
* Contributors: Johannes Meyer
0.3.0 (2013-09-11)
------------------
* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack
* decreased z_velocity_max to 2.0 m/s in logitech_gamepad.launch

89
Flight_control/src/enemy_suv/enemy_suv_teleop/CMakeLists.txt Normal file
View File

@ -0,0 +1,89 @@
cmake_minimum_required(VERSION 2.8.3)
project(enemy_suv_teleop)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs hector_uav_msgs)
include_directories(include ${catkin_INCLUDE_DIRS})
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS
LIBRARIES
CATKIN_DEPENDS roscpp sensor_msgs geometry_msgs hector_uav_msgs
DEPENDS
)
###########
## Build ##
###########
## Declare a cpp library
# add_library(@{name}
# src/${PROJECT_NAME}/@name.cpp
# )
## Declare a cpp executable
add_executable(enemy_suv_teleop src/quadrotor_teleop.cpp)
target_link_libraries(enemy_suv_teleop ${catkin_LIBRARIES})
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_dependencies(enemy_suv_teleop ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS enemy_suv_teleop
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

25
Flight_control/src/enemy_suv/enemy_suv_teleop/launch/logitech_gamepad.launch Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<launch>
<arg name="joy_dev" default="/dev/input/js0" />
<arg name="control_mode" default="twist" />
<node name="joy" pkg="joy" type="joy_node" output="screen" >
<param name="dev" value="$(arg joy_dev)" />
</node>
<!-- Note that axis IDs are those from the joystick message plus one, to be able to invert axes by specifiying either positive or negative axis numbers.-->
<!-- Axis 2 from joy message thus has to be set as '3' or '-3'(inverted mode) below-->
<node name="quadrotor_teleop" pkg="hector_quadrotor_teleop" type="quadrotor_teleop">
<param name="control_mode" value="$(arg control_mode)" />
<param name="x_axis" value="4"/>
<param name="y_axis" value="3"/>
<param name="z_axis" value="2"/>
<param name="yaw_axis" value="1"/>
<param name="x_velocity_max" value="5"/>
<param name="y_velocity_max" value="5"/>
<param name="z_velocity_max" value="2"/>
<param name="slow_button" value="6"/>
</node>
</launch>

19
Flight_control/src/enemy_suv/enemy_suv_teleop/launch/xbox_controller.launch Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<launch>
<arg name="joy_dev" default="/dev/input/js0" />
<node name="joy" pkg="joy" type="joy_node" output="screen" >
<param name="dev" value="$(arg joy_dev)" />
</node>
<!-- Note that axis IDs are those from the joystick message plus one, to be able to invert axes by specifiying either positive or negative axis numbers.-->
<!-- Axis 2 from joy message thus has to be set as '3' or '-3'(inverted mode) below-->
<node name="quadrotor_teleop" pkg="hector_quadrotor_teleop" type="quadrotor_teleop">
<param name="x_axis" value="5"/>
<param name="y_axis" value="4"/>
<param name="z_axis" value="2"/>
<param name="yaw_axis" value="1"/>
</node>
</launch>

33
Flight_control/src/enemy_suv/enemy_suv_teleop/package.xml Normal file
View File

@ -0,0 +1,33 @@
<package>
<name>enemy_suv_teleop</name>
<version>0.3.5</version>
<description>hector_quadrotor_teleop enables quadrotor flying with a joystick by
processing joy/Joy messages and translating them to geometry_msgs/Twist.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_teleop</url>
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>hector_uav_msgs</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>hector_uav_msgs</run_depend>
<run_depend>joy</run_depend>
<!-- Dependencies needed only for running tests. -->
</package>

215
Flight_control/src/enemy_suv/enemy_suv_teleop/src/quadrotor_teleop.cpp Normal file
View File

@ -0,0 +1,215 @@
//=================================================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
#include <hector_uav_msgs/YawrateCommand.h>
#include <hector_uav_msgs/ThrustCommand.h>
#include <hector_uav_msgs/AttitudeCommand.h>
namespace hector_quadrotor
{
class Teleop
{
private:
ros::NodeHandle node_handle_;
ros::Subscriber joy_subscriber_;
ros::Publisher velocity_publisher_, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
geometry_msgs::Twist velocity_;
hector_uav_msgs::AttitudeCommand attitude_;
hector_uav_msgs::ThrustCommand thrust_;
hector_uav_msgs::YawrateCommand yawrate_;
struct Axis
{
int axis;
double max;
};
struct Button
{
int button;
};
struct
{
Axis x;
Axis y;
Axis z;
Axis yaw;
} axes_;
struct
{
Button slow;
} buttons_;
double slow_factor_;
public:
Teleop()
{
ros::NodeHandle params("~");
params.param<int>("x_axis", axes_.x.axis, 4);
params.param<int>("y_axis", axes_.y.axis, 3);
params.param<int>("z_axis", axes_.z.axis, 2);
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
params.param<double>("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0);
params.param<int>("slow_button", buttons_.slow.button, 1);
params.param<double>("slow_factor", slow_factor_, 0.2);
std::string control_mode_str;
params.param<std::string>("control_mode", control_mode_str, "twist");
if (control_mode_str == "twist")
{
params.param<double>("x_velocity_max", axes_.x.max, 2.0);
params.param<double>("y_velocity_max", axes_.y.max, 2.0);
params.param<double>("z_velocity_max", axes_.z.max, 2.0);
joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
}
else if (control_mode_str == "attitude")
{
params.param<double>("x_roll_max", axes_.x.max, 0.35);
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
}
}
~Teleop()
{
stop();
}
void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
{
velocity_.linear.x = getAxis(joy, axes_.x);
velocity_.linear.y = getAxis(joy, axes_.y);
velocity_.linear.z = getAxis(joy, axes_.z);
velocity_.angular.z = getAxis(joy, axes_.yaw);
if (getButton(joy, buttons_.slow.button))
{
velocity_.linear.x *= slow_factor_;
velocity_.linear.y *= slow_factor_;
velocity_.linear.z *= slow_factor_;
velocity_.angular.z *= slow_factor_;
}
velocity_publisher_.publish(velocity_);
}
void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
{
attitude_.roll = getAxis(joy, axes_.x);
attitude_.pitch = getAxis(joy, axes_.y);
attitude_publisher_.publish(attitude_);
thrust_.thrust = getAxis(joy, axes_.z);
thrust_publisher_.publish(thrust_);
yawrate_.turnrate = getAxis(joy, axes_.yaw);
if (getButton(joy, buttons_.slow.button))
{
yawrate_.turnrate *= slow_factor_;
}
yawrate_publisher_.publish(yawrate_);
}
sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
{
if (axis.axis == 0)
{return 0;}
sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
if (axis.axis < 0)
{
sign = -1.0;
axis.axis = -axis.axis;
}
if ((size_t) axis.axis > joy->axes.size())
{return 0;}
return sign * joy->axes[axis.axis - 1] * axis.max;
}
sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
{
if (button <= 0)
{return 0;}
if ((size_t) button > joy->buttons.size())
{return 0;}
return joy->buttons[button - 1];
}
void stop()
{
if(velocity_publisher_.getNumSubscribers() > 0)
{
velocity_ = geometry_msgs::Twist();
velocity_publisher_.publish(velocity_);
}
if(attitude_publisher_.getNumSubscribers() > 0)
{
attitude_ = hector_uav_msgs::AttitudeCommand();
attitude_publisher_.publish(attitude_);
}
if(thrust_publisher_.getNumSubscribers() > 0)
{
thrust_ = hector_uav_msgs::ThrustCommand();
thrust_publisher_.publish(thrust_);
}
if(yawrate_publisher_.getNumSubscribers() > 0)
{
yawrate_ = hector_uav_msgs::YawrateCommand();
yawrate_publisher_.publish(yawrate_);
}
}
};
} // namespace hector_quadrotor
int main(int argc, char **argv)
{
ros::init(argc, argv, "quadrotor_teleop");
hector_quadrotor::Teleop teleop;
ros::spin();
return 0;
}

89
Flight_control/src/enemy_suv/enemy_suv_teleop_key/CMakeLists.txt Normal file
View File

@ -0,0 +1,89 @@
cmake_minimum_required(VERSION 2.8.3)
project(enemy_suv_teleop_key)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs)
include_directories(include ${catkin_INCLUDE_DIRS})
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS
LIBRARIES
CATKIN_DEPENDS roscpp geometry_msgs
DEPENDS
)
###########
## Build ##
###########
## Declare a cpp library
# add_library(@{name}
# src/${PROJECT_NAME}/@name.cpp
# )
## Declare a cpp executable
add_executable(enemy_suv_teleop_key src/enemy_suv_teleop_key.cpp)
target_link_libraries(enemy_suv_teleop_key ${catkin_LIBRARIES})
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
add_dependencies(enemy_suv_teleop_key ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS enemy_suv_teleop_key
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_5">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<!-- <param name="cmd_vel_robot" value="/robot_5/cmd_vel"/> -->
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

7
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key1.launch Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node>
</launch>

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key_0.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_0">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key_2.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_2">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key_3.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_3">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key_4.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_4">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

12
Flight_control/src/enemy_suv/enemy_suv_teleop_key/launch/key_5.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<!-- key to control hector quadrotor -->
<group ns="robot_5">
<node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key" output="screen">
<param name="cmd_vel_robot" value="/robot_5/cmd_vel"/>
</node>
</group>
<!-- <node pkg="hector_quadrotor_teleop_key" type="quadrotor_teleop_key" name="hector_quadrotor_teleop_key1" output="screen">
</node> -->
</launch>

27
Flight_control/src/enemy_suv/enemy_suv_teleop_key/package.xml Normal file
View File

@ -0,0 +1,27 @@
<package>
<name>enemy_suv_teleop_key</name>
<version>0.3.5</version>
<description>hector_quadrotor_teleop enables quadrotor flying with key translating them to geometry_msgs/Twist.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_teleop</url>
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend>
<!-- Dependencies needed only for running tests. -->
</package>

382
Flight_control/src/enemy_suv/enemy_suv_teleop_key/src/enemy_suv_teleop_key.cpp Normal file
View File

@ -0,0 +1,382 @@
//=================================================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <ros/ros.h>
// #include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
// #include <hector_uav_msgs/YawrateCommand.h>
// #include <hector_uav_msgs/ThrustCommand.h>
// #include <hector_uav_msgs/AttitudeCommand.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
#define KEYCODE_R 0x43
#define KEYCODE_L 0x44
#define KEYCODE_F 0x41
#define KEYCODE_B 0x42
#define KEYCODE_Q 0x71
int kfd = 0;
struct termios cooked, raw;
void quit(int sig)
{
(void)sig;
tcsetattr(kfd, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}
namespace hector_quadrotor
{
class Teleop
{
private:
ros::NodeHandle node_handle_;
// ros::Subscriber joy_subscriber_;
ros::Publisher velocity_publisher_; //, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
geometry_msgs::Twist velocity_;
// hector_uav_msgs::AttitudeCommand attitude_;
// hector_uav_msgs::ThrustCommand thrust_;
// hector_uav_msgs::YawrateCommand yawrate_;
struct Axis
{
int axis;
double max;
};
// struct Button
// {
// int button;
// };
struct
{
Axis x;
Axis y;
Axis z;
Axis yaw;
} axes_;
// struct
// {
// Button slow;
// } buttons_;
// double slow_factor_;
public:
Teleop()
{
ros::NodeHandle params("~");
// double cmd_vel_robot;
// ros::param::get("~cmd_vel_robot",cmd_vel_robot);
params.param<int>("x_axis", axes_.x.axis, 4);
params.param<int>("y_axis", axes_.y.axis, 3);
params.param<int>("z_axis", axes_.z.axis, 2);
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
params.param<double>("yaw_velocity_max", axes_.yaw.max, 0.0); // 90.0 * M_PI / 180.0
// params.param<int>("slow_button", buttons_.slow.button, 1);
// params.param<double>("slow_factor", slow_factor_, 0.2);
std::string control_mode_str;
params.param<std::string>("control_mode", control_mode_str, "twist");
if (control_mode_str == "twist")
{
params.param<double>("x_velocity_max", axes_.x.max, 0.0); //2.0
params.param<double>("y_velocity_max", axes_.y.max, 0.0); //2.0
params.param<double>("z_velocity_max", axes_.z.max, 0.0); //2.0
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
else if (control_mode_str == "attitude")
{
params.param<double>("x_roll_max", axes_.x.max, 0.35);
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
// attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
// yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
// thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
}
}
~Teleop()
{
stop();
}
void keyLoop()
{
char c;
bool dirty = false;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &= ~(ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("---------------------------");
puts("Use arrow keys and w a s d to move the quadrotor.");
for (;;)
{
// get the next event from the keyboard
if (read(kfd, &c, 1) < 0)
{
perror("read():");
exit(-1);
}
// linear_ = angular_ = 0;
ROS_DEBUG("value: 0x%02X\n", c);
switch (c)
{
case KEYCODE_W:
ROS_DEBUG("UP");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 2.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_S:
ROS_DEBUG("DOWN");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = -2.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_A:
ROS_DEBUG("TURN_LEFT");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_D:
ROS_DEBUG("TURN_RIGHT");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = -90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_L:
ROS_DEBUG("LEFT");
axes_.x.max = 20.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 1.0;//90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
axes_.x.max = 20.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = -1.0;//-90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_F:
ROS_DEBUG("FORWARD");
axes_.x.max = 20.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_B:
ROS_DEBUG("BACKWARD");
axes_.x.max = -20.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_Q:
ROS_DEBUG("STOP");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
default:
// max_tv = walk_vel_;
// max_rv = yaw_rate_;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
axes_.x.max = 0.0;
axes_.y.max = 0.0;
dirty = false;
}
// geometry_msgs::Twist twist;
velocity_.linear.x = axes_.x.max;
// velocity_.linear.y = axes_.y.max;
// velocity_.linear.z = axes_.z.max;
velocity_.angular.z = axes_.yaw.max;
// twist.angular.z = a_scale_ * angular_;
// twist.linear.x = l_scale_ * linear_;
if (dirty == true)
{
velocity_publisher_.publish(velocity_);
dirty = false;
}
}
return;
}
// void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
// {
// velocity_.linear.x = getAxis(joy, axes_.x);
// velocity_.linear.y = getAxis(joy, axes_.y);
// velocity_.linear.z = getAxis(joy, axes_.z);
// velocity_.angular.z = getAxis(joy, axes_.yaw);
// if (getButton(joy, buttons_.slow.button))
// {
// velocity_.linear.x *= slow_factor_;
// velocity_.linear.y *= slow_factor_;
// velocity_.linear.z *= slow_factor_;
// velocity_.angular.z *= slow_factor_;
// }
// velocity_publisher_.publish(velocity_);
// }
// void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
// {
// attitude_.roll = getAxis(joy, axes_.x);
// attitude_.pitch = getAxis(joy, axes_.y);
// attitude_publisher_.publish(attitude_);
// thrust_.thrust = getAxis(joy, axes_.z);
// thrust_publisher_.publish(thrust_);
// yawrate_.turnrate = getAxis(joy, axes_.yaw);
// if (getButton(joy, buttons_.slow.button))
// {
// yawrate_.turnrate *= slow_factor_;
// }
// yawrate_publisher_.publish(yawrate_);
// }
// sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
// {
// if (axis.axis == 0)
// {
// return 0;
// }
// sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
// if (axis.axis < 0)
// {
// sign = -1.0;
// axis.axis = -axis.axis;
// }
// if ((size_t)axis.axis > joy->axes.size())
// {
// return 0;
// }
// return sign * joy->axes[axis.axis - 1] * axis.max;
// }
// sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
// {
// if (button <= 0)
// {
// return 0;
// }
// if ((size_t)button > joy->buttons.size())
// {
// return 0;
// }
// return joy->buttons[button - 1];
// }
void stop()
{
if (velocity_publisher_.getNumSubscribers() > 0)
{
velocity_ = geometry_msgs::Twist();
velocity_publisher_.publish(velocity_);
}
// if (attitude_publisher_.getNumSubscribers() > 0)
// {
// attitude_ = hector_uav_msgs::AttitudeCommand();
// attitude_publisher_.publish(attitude_);
// }
// if (thrust_publisher_.getNumSubscribers() > 0)
// {
// thrust_ = hector_uav_msgs::ThrustCommand();
// thrust_publisher_.publish(thrust_);
// }
// if (yawrate_publisher_.getNumSubscribers() > 0)
// {
// yawrate_ = hector_uav_msgs::YawrateCommand();
// yawrate_publisher_.publish(yawrate_);
// }
}
};
} // namespace hector_quadrotor
int main(int argc, char **argv)
{
ros::init(argc, argv, "quadrotor_teleop_key");
hector_quadrotor::Teleop teleop;
// ros::spin();
signal(SIGINT, quit);
teleop.keyLoop();
return 0;
}

417
Flight_control/src/enemy_suv/enemy_suv_teleop_key/src/quadrotor_teleop_key_modify.cpp Normal file
View File

@ -0,0 +1,417 @@
//=================================================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <ros/ros.h>
// #include <sensor_msgs/Joy.h>
#include <geometry_msgs/Twist.h>
// #include <hector_uav_msgs/YawrateCommand.h>
// #include <hector_uav_msgs/ThrustCommand.h>
// #include <hector_uav_msgs/AttitudeCommand.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include <sys/poll.h>
#include <boost/thread/thread.hpp>
#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
#define KEYCODE_R 0x43
#define KEYCODE_L 0x44
#define KEYCODE_F 0x41
#define KEYCODE_B 0x42
#define KEYCODE_Q 0x71
int kfd = 0;
struct termios cooked, raw;
void quit(int sig)
{
(void)sig;
tcsetattr(kfd, TCSANOW, &cooked);
ros::shutdown();
exit(0);
}
namespace hector_quadrotor
{
class Teleop
{
private:
ros::NodeHandle node_handle_;
// ros::Subscriber joy_subscriber_;
ros::Publisher velocity_publisher_; //, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
geometry_msgs::Twist velocity_;
// hector_uav_msgs::AttitudeCommand attitude_;
// hector_uav_msgs::ThrustCommand thrust_;
// hector_uav_msgs::YawrateCommand yawrate_;
struct Axis
{
int axis;
double max;
};
// struct Button
// {
// int button;
// };
struct
{
Axis x;
Axis y;
Axis z;
Axis yaw;
} axes_;
// struct
// {
// Button slow;
// } buttons_;
// double slow_factor_;
public:
Teleop()
{
ros::NodeHandle params("~");
params.param<int>("x_axis", axes_.x.axis, 4);
params.param<int>("y_axis", axes_.y.axis, 3);
params.param<int>("z_axis", axes_.z.axis, 2);
params.param<int>("yaw_axis", axes_.yaw.axis, 1);
params.param<double>("yaw_velocity_max", axes_.yaw.max, 0.0); // 90.0 * M_PI / 180.0
// params.param<int>("slow_button", buttons_.slow.button, 1);
// params.param<double>("slow_factor", slow_factor_, 0.2);
std::string control_mode_str;
params.param<std::string>("control_mode", control_mode_str, "twist");
if (control_mode_str == "twist")
{
params.param<double>("x_velocity_max", axes_.x.max, 0.0); //2.0
params.param<double>("y_velocity_max", axes_.y.max, 0.0); //2.0
params.param<double>("z_velocity_max", axes_.z.max, 0.0); //2.0
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
else if (control_mode_str == "attitude")
{
params.param<double>("x_roll_max", axes_.x.max, 0.35);
params.param<double>("y_pitch_max", axes_.y.max, 0.35);
params.param<double>("z_thrust_max", axes_.z.max, 25.0);
// joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
// attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
// yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
// thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
}
}
~Teleop()
{
stop();
}
void keyLoop()
{
char c;
bool dirty = false;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &= ~(ICANON | ECHO);
// Setting a new line, then end of file
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("---------------------------");
puts("Use arrow keys and w a s d to move the quadrotor.");
struct pollfd ufd;
ufd.fd = kfd;
ufd.events = POLLIN;
for (;;)
{
boost::this_thread::interruption_point();
// get the next event from the keyboard
int num;
if ((num = poll(&ufd, 1, 250)) < 0)
{
perror("poll():");
return;
}
else if (num > 0)
{
if (read(kfd, &c, 1) < 0)
{
perror("read():");
return;
}
}
else
{
if (dirty == true)
{
stopRobot();
dirty = false;
}
continue;
}
for (;;)
{
// get the next event from the keyboard
if (read(kfd, &c, 1) < 0)
{
perror("read():");
exit(-1);
}
// linear_ = angular_ = 0;
ROS_DEBUG("value: 0x%02X\n", c);
switch (c)
{
case KEYCODE_W:
ROS_DEBUG("UP");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 2.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_S:
ROS_DEBUG("DOWN");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = -2.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_A:
ROS_DEBUG("TURN_LEFT");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_D:
ROS_DEBUG("TURN_RIGHT");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = -90.0 * M_PI / 180.0;
dirty = true;
break;
case KEYCODE_L:
ROS_DEBUG("LEFT");
axes_.x.max = 2.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_R:
ROS_DEBUG("RIGHT");
axes_.x.max = -2.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_F:
ROS_DEBUG("FORWARD");
axes_.x.max = 0.0;
axes_.y.max = 2.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_B:
ROS_DEBUG("BACKWARD");
axes_.x.max = 0.0;
axes_.y.max = -2.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
case KEYCODE_Q:
ROS_DEBUG("BACKWARD");
axes_.x.max = 0.0;
axes_.y.max = 0.0;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
dirty = true;
break;
default:
// max_tv = walk_vel_;
// max_rv = yaw_rate_;
axes_.z.max = 0.0;
axes_.yaw.max = 0.0;
axes_.x.max = 0.0;
axes_.y.max = 0.0;
dirty = false;
}
// geometry_msgs::Twist twist;
velocity_.linear.x = axes_.x.max;
velocity_.linear.y = axes_.y.max;
velocity_.linear.z = axes_.z.max;
velocity_.angular.z = axes_.yaw.max;
// twist.angular.z = a_scale_ * angular_;
// twist.linear.x = l_scale_ * linear_;
if (dirty == true)
{
velocity_publisher_.publish(velocity_);
dirty = false;
}
}
return;
}
// void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
// {
// velocity_.linear.x = getAxis(joy, axes_.x);
// velocity_.linear.y = getAxis(joy, axes_.y);
// velocity_.linear.z = getAxis(joy, axes_.z);
// velocity_.angular.z = getAxis(joy, axes_.yaw);
// if (getButton(joy, buttons_.slow.button))
// {
// velocity_.linear.x *= slow_factor_;
// velocity_.linear.y *= slow_factor_;
// velocity_.linear.z *= slow_factor_;
// velocity_.angular.z *= slow_factor_;
// }
// velocity_publisher_.publish(velocity_);
// }
// void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
// {
// attitude_.roll = getAxis(joy, axes_.x);
// attitude_.pitch = getAxis(joy, axes_.y);
// attitude_publisher_.publish(attitude_);
// thrust_.thrust = getAxis(joy, axes_.z);
// thrust_publisher_.publish(thrust_);
// yawrate_.turnrate = getAxis(joy, axes_.yaw);
// if (getButton(joy, buttons_.slow.button))
// {
// yawrate_.turnrate *= slow_factor_;
// }
// yawrate_publisher_.publish(yawrate_);
// }
// sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
// {
// if (axis.axis == 0)
// {
// return 0;
// }
// sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
// if (axis.axis < 0)
// {
// sign = -1.0;
// axis.axis = -axis.axis;
// }
// if ((size_t)axis.axis > joy->axes.size())
// {
// return 0;
// }
// return sign * joy->axes[axis.axis - 1] * axis.max;
// }
// sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
// {
// if (button <= 0)
// {
// return 0;
// }
// if ((size_t)button > joy->buttons.size())
// {
// return 0;
// }
// return joy->buttons[button - 1];
// }
void stop()
{
if (velocity_publisher_.getNumSubscribers() > 0)
{
velocity_ = geometry_msgs::Twist();
velocity_publisher_.publish(velocity_);
}
// if (attitude_publisher_.getNumSubscribers() > 0)
// {
// attitude_ = hector_uav_msgs::AttitudeCommand();
// attitude_publisher_.publish(attitude_);
// }
// if (thrust_publisher_.getNumSubscribers() > 0)
// {
// thrust_ = hector_uav_msgs::ThrustCommand();
// thrust_publisher_.publish(thrust_);
// }
// if (yawrate_publisher_.getNumSubscribers() > 0)
// {
// yawrate_ = hector_uav_msgs::YawrateCommand();
// yawrate_publisher_.publish(yawrate_);
// }
}
};
} // namespace hector_quadrotor
int
main(int argc, char **argv)
{
ros::init(argc, argv, "quadrotor_teleop_key");
hector_quadrotor::Teleop teleop;
// ros::spin();
signal(SIGINT, quit);
teleop.keyLoop();
return 0;
}

123
Flight_control/src/enemy_suv/suv_move/CMakeLists.txt Normal file
View File

@ -0,0 +1,123 @@
cmake_minimum_required(VERSION 2.8.3)
project(suv_move)
set(CMAKE_CXX_STANDARD 11)
if (NOT CMAKE_BUILD_TYPE)
# Options: Debug, Release, MinSizeRel, RelWithDebInfo
message(STATUS "No build type selected, default to Release")
set(CMAKE_BUILD_TYPE "Release")
endif()
set(CMAKE_CXX_FLAGS "-fopenmp")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
rosflight_msgs
rosplane_msgs
dynamic_reconfigure
sensor_msgs
tf
)
find_package(Eigen3 REQUIRED)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
# Generate dynamic reconfigure parameters in the 'cfg' folder
#generate_dynamic_reconfigure_options(
# cfg/Follower.cfg
# cfg/Controller.cfg
#)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES
CATKIN_DEPENDS roscpp rospy rosflight_msgs rosplane_msgs
# DEPENDS system_lib
)
include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
## Declare a C++ executable
#add_executable(rosplane_controller
# src/controller_base.cpp
# src/controller_example.cpp)
#add_dependencies(rosplane_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
#target_link_libraries(rosplane_controller ${catkin_LIBRARIES})
#add_executable(rosplane_controller99
# src/controller_base99.cpp
# src/controller_example99.cpp)
#add_dependencies(rosplane_controller99 ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
#target_link_libraries(rosplane_controller99 ${catkin_LIBRARIES})
add_executable(suv_pseudo_controller
# src/controller_base.cpp
# src/controller_example.cpp)
src/pseudo_controller_base.cpp
src/pseudo_controller_example.cpp)
add_dependencies(suv_pseudo_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries(suv_pseudo_controller ${catkin_LIBRARIES})
## Declare a C++ executable
#add_executable(rosplane_estimator
# src/estimator_base.cpp
# src/estimator_example.cpp)
#add_dependencies(rosplane_estimator ${catkin_EXPORTED_TARGETS})
#target_link_libraries(rosplane_estimator ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(suv_estimator_statesub
src/estimator_base_statesub.cpp
)
add_dependencies(suv_estimator_statesub ${catkin_EXPORTED_TARGETS})
target_link_libraries(suv_estimator_statesub ${catkin_LIBRARIES})
## Declare a C++ executable
#add_executable(rosplane_estimator_updata_statesub
# src/estimator_base_updata_statesub.cpp
# )
#add_dependencies(rosplane_estimator_updata_statesub ${catkin_EXPORTED_TARGETS})
#target_link_libraries(rosplane_estimator_updata_statesub ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(suv_estimator_99pub
src/estimator_base_99pub.cpp
)
add_dependencies(suv_estimator_99pub ${catkin_EXPORTED_TARGETS})
target_link_libraries(suv_estimator_99pub ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(suv_path_follower
src/path_follower_example.cpp
src/path_follower_base.cpp)
add_dependencies(suv_path_follower ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries(suv_path_follower ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(suv_path_manager
src/path_manager_base.cpp
src/path_manager_example.cpp)
add_dependencies(suv_path_manager ${catkin_EXPORTED_TARGETS})
target_link_libraries(suv_path_manager ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(suv_path_planner
src/path_planner.cpp)
add_dependencies(suv_path_planner ${catkin_EXPORTED_TARGETS})
target_link_libraries(suv_path_planner ${catkin_LIBRARIES})

58
Flight_control/src/enemy_suv/suv_move/cfg/Controller.cfg Normal file
View File

@ -0,0 +1,58 @@
#!/usr/bin/env python
PACKAGE = "rosplane"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# trim
trim = gen.add_group("Trim")
trim.add("TRIM_E", double_t, 0, "Elevator trim", 0, -1, 1)
trim.add("TRIM_A", double_t, 0, "Aileron trim", 0, -1, 1)
trim.add("TRIM_R", double_t, 0, "Rudder trim", 0, -1, 1)
trim.add("TRIM_T", double_t, 0, "Throttle trim", 0.6, 0, 1)
# course hold
course = gen.add_group("Course")
course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, 0, 2)
course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -1, 0)
course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.0, 0, 0.2)
# roll hold
roll = gen.add_group("Roll")
roll.add("ROLL_KP", double_t, 0, "Roll proportional gain", 1.17, 0, 3)
roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.13, -1, 0)
roll.add("ROLL_KI", double_t, 0, "Roll integral gain", 0, 0, 0.2)
# pitch hold
pitch = gen.add_group("Pitch")
pitch.add("PITCH_KP", double_t, 0, "Pitch proportional gain", 1.0, 0, 3)
pitch.add("PITCH_KD", double_t, 0, "Pitch derivative gain", -0.17, -0.4, 0)
pitch.add("PITCH_KI", double_t, 0, "Pitch integral gain", 0, 0, 0.2)
pitch.add("PITCH_FF", double_t, 0, "Pitch feed forward value", 0, -1, 1)
# airspeed with pitch hold
as_pitch = gen.add_group("Airspeed with Pitch")
as_pitch.add("AS_PITCH_KP", double_t, 0, "Airspeed with pitch proportional gain", -0.0713, 0, 0.2)
as_pitch.add("AS_PITCH_KD", double_t, 0, "Airspeed with pitch derivative gain", -0.0635, -0.2, 0)
as_pitch.add("AS_PITCH_KI", double_t, 0, "Airspeed with pitch integral gain", 0, 0, 0.2)
# airspeed with throttle hold
as_thr = gen.add_group("Airspeed with Throttle")
as_thr.add("AS_THR_KP", double_t, 0, "Airspeed with throttle proportional gain", 3.2, 0, 10)
as_thr.add("AS_THR_KD", double_t, 0, "Airspeed with throttle derivative gain", 0, -5, 0)
as_thr.add("AS_THR_KI", double_t, 0, "Airspeed with throttle integral gain", 1.0, 0, 10)
# altitude hold
alt = gen.add_group("Altitude")
alt.add("ALT_KP", double_t, 0, "Altitude proportional gain", 0.045, 0, 0.1)
alt.add("ALT_KD", double_t, 0, "Altitude derivative gain", 0, -0.05, 0)
alt.add("ALT_KI", double_t, 0, "Altitude integral gain", 0.01, 0, 0.05)
# side-slip hold
sideslip = gen.add_group("Side Slip")
sideslip.add("BETA_KP", double_t, 0, "Side slip proportional gain", -0.1164, 0, 0.3)
sideslip.add("BETA_KD", double_t, 0, "Side slip derivative gain", 0, -0.15, 0)
sideslip.add("BETA_KI", double_t, 0, "Side slip integral gain", -0.0037111, 0, 0.05)
exit(gen.generate(PACKAGE, "rosplane", "Controller"))

17
Flight_control/src/enemy_suv/suv_move/cfg/Follower.cfg Normal file
View File

@ -0,0 +1,17 @@
#!/usr/bin/env python
PACKAGE = "rosplane"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Chi Infinity
gen.add("CHI_INFTY", double_t, 0, "Chi Infinity", 1.0472, 0 , 1.5708)
# K Path
gen.add("K_PATH", double_t, 0, "K Path", 0.025, 0, 1)
# K Orbit
gen.add("K_ORBIT", double_t, 0, "K Orbit", 4.0, 0, 15)
exit(gen.generate(PACKAGE, "rosplane", "Follower"))

145
Flight_control/src/enemy_suv/suv_move/include/controller_base.h Normal file
View File

@ -0,0 +1,145 @@
/**
* @file controller_base.h
*
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef CONTROLLER_BASE_H
#define CONTROLLER_BASE_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Controller_Commands.h>
#include <rosplane_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <rosplane/ControllerConfig.h>
namespace rosplane
{
enum class alt_zones
{
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD
};
class controller_base
{
public:
controller_base();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float psi; //
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};
struct output_s
{
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
float delta_e; //升降舵偏移量-拉高降低
float delta_r; //方向舵偏移量-保持稳定，消除测滑
float delta_t; //油门偏移量
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher actuators_pub_;
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
rosplane_msgs::Controller_Commands controller_commands_;
rosplane_msgs::State vehicle_state_;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
/**
* Convert from deflection angle to pwm
*/
void convert_to_pwm(struct output_s &output);
/**
* Publish the outputs
*/
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // CONTROLLER_BASE_H

145
Flight_control/src/enemy_suv/suv_move/include/controller_base99.h Normal file
View File

@ -0,0 +1,145 @@
/**
* @file controller_base99.h
*
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef CONTROLLER_BASE99_H
#define CONTROLLER_BASE99_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Controller_Commands.h>
#include <rosplane_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <rosplane/ControllerConfig.h>
namespace rosplane
{
enum class alt_zones
{
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD
};
class controller_base99
{
public:
controller_base99();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float psi; //
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};
struct output_s
{
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
float delta_e; //升降舵偏移量-拉高降低
float delta_r; //方向舵偏移量-保持稳定，消除测滑
float delta_t; //油门偏移量
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher actuators_pub_;
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
rosplane_msgs::Controller_Commands controller_commands_;
rosplane_msgs::State vehicle_state_;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
/**
* Convert from deflection angle to pwm
*/
void convert_to_pwm(struct output_s &output);
/**
* Publish the outputs
*/
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // CONTROLLER_BASE99_H

53
Flight_control/src/enemy_suv/suv_move/include/controller_example.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef CONTROLLER_EXAMPLE_H
#define CONTROLLER_EXAMPLE_H
#include "controller_base.h"
namespace rosplane
{
class controller_example : public controller_base
{
public:
controller_example();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error_;
float r_integrator;
float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error_;
float p_integrator_;
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // CONTROLLER_EXAMPLE_H

56
Flight_control/src/enemy_suv/suv_move/include/controller_example99.h Normal file
View File

@ -0,0 +1,56 @@
#ifndef CONTROLLER_EXAMPLE99_H
#define CONTROLLER_EXAMPLE99_H
#include "controller_base99.h"
namespace rosplane
{
class controller_example99 : public controller_base99
{
public:
controller_example99();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error_;
float r_integrator;
float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error_;
float p_integrator_;
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
float decend_airspeed_with_pitch_hold(float h_c, float h, const struct params_s &params, float Ts);
float decend_airspeed_oil(float h_c, float h, const params_s &params, float Ts);
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // CONTROLLER_EXAMPLE99_H

131
Flight_control/src/enemy_suv/suv_move/include/estimator_base.h Normal file
View File

@ -0,0 +1,131 @@
/**
* @file estimator_base.h
*
* Base class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef ESTIMATOR_BASE_H
#define ESTIMATOR_BASE_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosflight_msgs/GPS.h>
#include <sensor_msgs/Imu.h>
#include <rosflight_msgs/Barometer.h>
#include <rosflight_msgs/Airspeed.h>
#include <rosflight_msgs/Status.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
class estimator_base
{
public:
estimator_base();
protected:
struct input_s
{
float gyro_x;
float gyro_y;
float gyro_z;
float accel_x;
float accel_y;
float accel_z;
float static_pres;
float diff_pres;
bool gps_new;
float gps_n;
float gps_e;
float gps_h;
float gps_Vg;
float gps_course;
bool status_armed;
bool armed_init;
};
struct output_s
{
float pn;
float pe;
float h;
float Va;
float alpha;
float beta;
float phi;
float theta;
float psi;
float chi;
float p;
float q;
float r;
float Vg;
float wn;
float we;
};
struct params_s
{
double gravity;
double rho;
double sigma_accel;
double sigma_n_gps;
double sigma_e_gps;
double sigma_Vg_gps;
double sigma_course_gps;
double Ts;
};
virtual void estimate(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber gps_sub_;
ros::Subscriber imu_sub_;
ros::Subscriber baro_sub_;
ros::Subscriber airspeed_sub_;
ros::Subscriber status_sub_;
void update(const ros::TimerEvent &);
void gpsCallback(const rosflight_msgs::GPS &msg);
void imuCallback(const sensor_msgs::Imu &msg);
void baroAltCallback(const rosflight_msgs::Barometer &msg);
void airspeedCallback(const rosflight_msgs::Airspeed &msg);
void statusCallback(const rosflight_msgs::Status &msg);
double update_rate_;
ros::Timer update_timer_;
std::string gps_topic_;
std::string imu_topic_;
std::string baro_topic_;
std::string airspeed_topic_;
std::string status_topic_;
bool gps_new_;
bool gps_init_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool armed_first_time_; /**< Arm before starting estimation */
bool baro_init_; /**< Initial barometric pressure */
float init_static_; /**< Initial static pressure (mbar) */
int baro_count_; /**< Used to grab the first set of baro measurements */
std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
struct params_s params_;
struct input_s input_;
};
} //end namespace
#endif // ESTIMATOR_BASE_H

45
Flight_control/src/enemy_suv/suv_move/include/estimator_base_99pub.h Normal file
View File

@ -0,0 +1,45 @@
// modified by kobe and zhangshuai
#ifndef ESTIMATOR_BASE_PUB_H
#define ESTIMATOR_BASE_PUB_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/State29.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
class estimator_base_99pub
{
public:
estimator_base_99pub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state99_pub_;
ros::Subscriber true_state_sub_;
void update(const ros::TimerEvent &);
void truestateCallback(const rosplane_msgs::StateConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string truth_topic_;
bool state_init_;
rosplane_msgs::State true_state_;
};
} //end namespace
#endif //

46
Flight_control/src/enemy_suv/suv_move/include/estimator_base_statesub.h Normal file
View File

@ -0,0 +1,46 @@
// modified by kobe and zhangshuai
#ifndef ESTIMATOR_BASE_SUB_H
#define ESTIMATOR_BASE_SUB_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/State29.h> //采用新的消息头文件
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
class estimator_base_statesub
{
public:
estimator_base_statesub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber state29_sub_;
void update(const ros::TimerEvent &);
void state99Callback(const rosplane_msgs::State29ConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string state29_topic_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool state_init_;
rosplane_msgs::State29 state29_;
};
} //end namespace
#endif //

46
Flight_control/src/enemy_suv/suv_move/include/estimator_base_updata_statesub.h Normal file
View File

@ -0,0 +1,46 @@
// modified by kobe
#ifndef ESTIMATOR_BASE_UPDATA_SUB_H
#define ESTIMATOR_BASE_UPDATA_SUB_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Up_Data_New.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
class estimator_base_updata_statesub
{
public:
estimator_base_updata_statesub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber state29_sub_;
void update(const ros::TimerEvent &);
void state99Callback(const rosplane_msgs::Up_Data_NewConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string state29_topic_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool state_init_;
rosplane_msgs::Up_Data_New state29_;
};
} //end namespace
#endif //

73
Flight_control/src/enemy_suv/suv_move/include/estimator_example.h Normal file
View File

@ -0,0 +1,73 @@
#ifndef ESTIMATOR_EXAMPLE_H
#define ESTIMATOR_EXAMPLE_H
#include "estimator_base.h"
#include <math.h>
#include <Eigen/Eigen>
namespace rosplane
{
class estimator_example : public estimator_base
{
public:
estimator_example();
private:
virtual void estimate(const params_s &params, const input_s &input, output_s &output);
// float gps_n_old_;
// float gps_e_old_;
// float gps_Vg_old_;
// float gps_course_old_;
double lpf_a_;
float alpha_;
float alpha1_;
int N_;
float lpf_gyro_x_;
float lpf_gyro_y_;
float lpf_gyro_z_;
float lpf_static_;
float lpf_diff_;
float lpf_accel_x_;
float lpf_accel_y_;
float lpf_accel_z_;
float phat_;
float qhat_;
float rhat_;
float Vwhat_;
float phihat_;
float thetahat_;
float psihat_;
Eigen::Vector2f xhat_a_; // 2
Eigen::Matrix2f P_a_; // 2x2
Eigen::VectorXf xhat_p_; // 7
Eigen::MatrixXf P_p_; // 7x7
Eigen::Matrix2f Q_a_; // 2x2
float R_accel_;
Eigen::Vector2f f_a_; // 2
Eigen::Matrix2f A_a_; // 2x2
float h_a_;
Eigen::Vector2f C_a_; // 2
Eigen::Vector2f L_a_; // 2
Eigen::MatrixXf Q_p_; // 7x7
Eigen::MatrixXf R_p_; // 6x6
Eigen::VectorXf f_p_; // 7
Eigen::MatrixXf A_p_; // 7x7
float h_p_;
Eigen::VectorXf C_p_; // 7
Eigen::VectorXf L_p_; // 7
void check_xhat_a();
};
} //end namespace
#endif // ESTIMATOR_EXAMPLE_H

103
Flight_control/src/enemy_suv/suv_move/include/path_follower_base.h Normal file
View File

@ -0,0 +1,103 @@
//* @author AIRC-DA group,questions contack kobe.
#ifndef PATH_FOLLOWER_BASE_H
#define PATH_FOLLOWER_BASE_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Controller_Commands.h>
#include <dynamic_reconfigure/server.h>
#include <rosplane/FollowerConfig.h>
#include <rosplane_msgs/Current_Path.h>
#include <nav_msgs/Odometry.h>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
enum class path_type
{
Orbit,
Line,
Star //add by kobe
};
class path_follower_base
{
public:
path_follower_base();
float spin();
protected:
struct input_s
{
enum path_type p_type;
float Va_d;
float r_path[3];
float q_path[3];
float c_orbit[3];
float rho_orbit;
int lam_orbit;
float pn; /** position north */
float pe; /** position east */
float h; /** altitude */
float Va; /** airspeed */
float chi; /** course angle */
float h_c_path; /** goal attitude */
float Va_c_path; /** goal attitude */
bool landing;
bool takeoff;
};
struct output_s
{
double Va_c; /** commanded airspeed (m/s) */
double h_c; /** commanded altitude (m) */
double chi_c; /** commanded course (rad) */
double phi_ff; /** feed forward term for orbits (rad) */
};
struct params_s
{
double chi_infty;
double k_path;
double k_orbit;
};
virtual void follow(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber bebop1_state_sub_;
ros::Subscriber current_path_sub_;
ros::Publisher controller_commands_pub_;
double update_rate_ = 100.0;
ros::Timer update_timer_;
rosplane_msgs::Controller_Commands controller_commands_;
struct params_s params_; /**< params */
struct input_s input_;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
bool state_init_;
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
void current_path_callback(const rosplane_msgs::Current_PathConstPtr &msg);
bool current_path_init_;
dynamic_reconfigure::Server<rosplane::FollowerConfig> server_;
dynamic_reconfigure::Server<rosplane::FollowerConfig>::CallbackType func_;
void reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level);
void update(const ros::TimerEvent &);
};
} // end namespace
#endif // PATH_FOLLOWER_BASE_H

18
Flight_control/src/enemy_suv/suv_move/include/path_follower_example.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef PATH_FOLLOWER_EXAMPLE_H
#define PATH_FOLLOWER_EXAMPLE_H
#include "path_follower_base.h"
namespace rosplane
{
class path_follower_example : public path_follower_base
{
public:
path_follower_example();
private:
virtual void follow(const struct params_s &params, const struct input_s &input, struct output_s &output);
};
} //end namespace
#endif // PATH_FOLLOWER_EXAMPLE_H

122
Flight_control/src/enemy_suv/suv_move/include/path_manager_base.h Normal file
View File

@ -0,0 +1,122 @@
//* @author AIRC-DA group,questions contack kobe.
#ifndef PATH_MANAGER_BASE_H
#define PATH_MANAGER_BASE_H
#include <ros/ros.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Current_Path.h>
#include <rosplane_msgs/Goal_Info.h>
#include <rosplane_msgs/Down_Data_New.h>
// #include <commond_msgs/Waypoint.h>
#include <rosplane_msgs/Waypoint.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/FluidPressure.h>
#include <math.h>
#include <Eigen/Eigen>
// #include <rosplane/ControllerConfig.h>
#include <nav_msgs/Odometry.h>
#define EARTH_RADIUS 6378145.0f
namespace rosplane
{
class path_manager_base
{
public:
path_manager_base();
protected:
//modified by kobe
struct waypoint_s
{
float w[3];
float lat;
float lon;
float chi_d;
bool chi_valid;
float Va_d;
bool landing;
bool takeoff;
};
std::vector<waypoint_s> waypoints_;
waypoint_s waypoint_start_;
waypoint_s waypoint_end_;
bool waypoint_received_;
int num_waypoints_;
int idx_a_; /** index to the waypoint that was most recently achieved */
float min_distance_;
double min_distance_titude;
bool show; //add by kobe
bool show2;
struct input_s
{
float pn; /** position north */
float pe; /** position east */
float lat; //latitude
float lon; //lontitude
float h; /** altitude */
float chi; /** course angle */
float va; //add by kobe
};
struct output_s
{
float Va_c;
int flag; /** Inicates strait line or orbital path (1 is line, 0 is orbit, 2 is star) modified by kobe*/
float Va_d; /** Desired airspeed (m/s) */
float r[3]; /** Vector to origin of straight line path (m) */
float q[3]; /** Unit vector, desired direction of travel for line path */
float c[3]; /** Center of orbital path (m) */
float gxy[2]; /** x-y-coordinate (m) */
float gll[2]; /** latitude-longtitude */
float rho; /** Radius of orbital path (m) */
float h_c;
bool landing;
bool takeoff;
int8_t lambda; /** Direction of orbital path (cw is 1, ccw is -1) */
};
struct params_s
{
double R_min;
};
virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_; /**< vehicle state subscription */
ros::Subscriber bebop1_state_sub_;
ros::Subscriber new_waypoint_sub_; /**< new waypoint subscription */
ros::Publisher current_path_pub_; /**< controller commands publication */
ros::Publisher goal_info_pub_; /**< goal info publication */
ros::Publisher down_data_pub_;
ros::Subscriber waypoint_received_sub_;
struct params_s params_;
rosplane_msgs::State vehicle_state_; /**< vehicle state */
nav_msgs::Odometry bebop1_state_;
double update_rate_;
ros::Timer update_timer_;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
bool state_init_;
void new_waypoint_callback(const rosplane_msgs::Waypoint &msg);
void current_path_publish(const ros::TimerEvent &);
void waypoint_received_callback(const rosplane_msgs::Waypoint &msg);
};
} //end namespace
#endif // PATH_MANAGER_BASE_H

78
Flight_control/src/enemy_suv/suv_move/include/path_manager_example.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef PATH_MANAGER_EXAMPLE_H
#define PATH_MANAGER_EXAMPLE_H
#include "path_manager_base.h"
#include <Eigen/Eigen>
//#include <lib/mathlib/mathlib.h>
#define M_PI_F 3.14159265358979323846f
#define M_PI_2_F 1.57079632679489661923f
namespace rosplane
{
enum class fillet_state
{
STRAIGHT,
ORBIT
};
enum class dubin_state
{
FIRST,
BEFORE_H1,
BEFORE_H1_WRONG_SIDE,
STRAIGHT,
BEFORE_H3,
BEFORE_H3_WRONG_SIDE
};
class path_manager_example : public path_manager_base
{
public:
path_manager_example();
private:
virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output);
void manage_line(const struct params_s &params, const struct input_s &input, struct output_s &output);
void manage_star(const struct params_s &params, const struct input_s &input, struct output_s &output);//add by kobe
void manage_fillet(const struct params_s &params, const struct input_s &input, struct output_s &output);
fillet_state fil_state_;
void manage_dubins(const struct params_s &params, const struct input_s &input, struct output_s &output);
dubin_state dub_state_;
double earth_distance(double, double, double, double);
bool judge_condition1;
bool judge_condition2;
bool judge_condition3;
bool judge_condition4;
bool judge_condition5;
struct dubinspath_s
{
Eigen::Vector3f ps; /** the start position */
float chis; /** the start course angle */
Eigen::Vector3f pe; /** the end position */
float chie; /** the end course angle */
float R; /** turn radius */
float L; /** length of the path */
Eigen::Vector3f cs; /** center of the start circle */
int lams; /** direction of the start circle */
Eigen::Vector3f ce; /** center of the endcircle */
int lame; /** direction of the end circle */
Eigen::Vector3f w1; /** vector defining half plane H1 */
Eigen::Vector3f q1; /** unit vector along striaght line path */
Eigen::Vector3f w2; /** vector defining half plane H2 */
Eigen::Vector3f w3; /** vector defining half plane H3 */
Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */
};
struct dubinspath_s dubinspath_;
void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R);
Eigen::Matrix3f rotz(float theta);
float mo(float in);
};
} //end namespace
#endif // PATH_MANAGER_EXAMPLE_H

137
Flight_control/src/enemy_suv/suv_move/include/pseudo_controller_base.h Normal file
View File

@ -0,0 +1,137 @@
/**
*
* Refer autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*pseudo fix-wing 四旋翼模拟固定翼飞行，订阅当前实际及命令的速度、高度和角度，发布向前，向上下速度和角度
* @author AIRC-DA group,questions contack kobe.
*/
#ifndef PSEUDO_CONTROLLER_BASE_H
#define PSEUDO_CONTROLLER_BASE_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <geometry_msgs/Twist.h>
#include <rosplane_msgs/State.h>
#include <rosplane_msgs/Controller_Commands.h>
#include <rosplane_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <rosplane/ControllerConfig.h>
#include <nav_msgs/Odometry.h>
namespace rosplane
{
enum class alt_zones
{
PREPARE,
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD,
LANDING
};
class pseudo_controller_base
{
public:
pseudo_controller_base();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float va; /** airspeed */
float h; /** altitude */
float chi; /** course angle */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (north)(-pi to pi,clockwise:-) */
bool takeoff;
bool landing;
};
struct output_s
{
//float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
//float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
//float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
//float delta_e; //升降舵偏移量-拉高降低
//float delta_r; //方向舵偏移量-保持稳定，消除测滑
//float delta_t; //油门偏移量
float forward_trans; //前进力度
float up_down_trans; //向上、向下力度,上正下负
float left_right_trans; //向左右力度,左正右负
float roll_trans; //左右滚转，x轴,逆时针为正，顺时针为负，[-pi/2,pi/2]
float rotate_trans; //z旋转,逆时针为正，顺时针为负，[-pi/2,pi/2]
float rotate_value;
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
//double c_kd;
double c_ki;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_ki;
double a_t_kp;
double a_t_kd;
double a_kp;
double a_kd;
double a_ki;
double trim_t;
//double max_a;y
double max_t;
};
bool reached;
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber bebop1_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher pesudors_pub_;//actuators
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
rosplane_msgs::Controller_Commands controller_commands_;
rosplane_msgs::State vehicle_state_;
nav_msgs::Odometry bebop1_state_;
void vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg);
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
void controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<rosplane::ControllerConfig> server_;
dynamic_reconfigure::Server<rosplane::ControllerConfig>::CallbackType func_;
void reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level);
/*Convert from deflection angle to pwm*/
//void convert_to_pwm(struct output_s &output);
/* Publish the outputs */
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // PSEUDO_CONTROLLER_BASE_H

66
Flight_control/src/enemy_suv/suv_move/include/pseudo_controller_example.h Normal file
View File

@ -0,0 +1,66 @@
#ifndef PSEUDO_CONTROLLER_EXAMPLE_H
#define PSEUDO_CONTROLLER_EXAMPLE_H
#include "pseudo_controller_base.h"
namespace rosplane
{
class pseudo_controller_example : public pseudo_controller_base
{
public:
pseudo_controller_example();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float pseudo_course_hold(float chi,float chi_c, const params_s &params, float Ts);
//float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float r_error_;
float r_integrator_;
//float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
//float r_error_;
//float r_integrator_;
//float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
//float p_error_;
//float p_integrator_;
float pseudo_course(float chi, float chi_c);
float pseudo_left_right(float chi, float chi_c, float forward_trans);
//float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float pseudo_pitch_hold(float h_c, float h, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
//float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float pseudo_throttle_hold(float Va_c, float Va, const params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
//float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float pseudo_altitiude_hold(float h_c, float h, const params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // PSEUDO_CONTROLLER_EXAMPLE_H

33
Flight_control/src/enemy_suv/suv_move/package.xml Normal file
View File

@ -0,0 +1,33 @@
<?xml version="1.0"?>
<package>
<name>suv_move</name>
<version>0.0.0</version>
<description>The suv_move package</description>
<maintainer email="gary.ellingson@byu.edu">Gary Ellingson</maintainer>
<author email="gary.ellingson@byu.edu">Gary Ellingson</author>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>rosflight_msgs</build_depend>
<build_depend>rosplane_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>rosflight_msgs</run_depend>
<run_depend>rosplane_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<export>
</export>
</package>

33
Flight_control/src/enemy_suv/suv_move/param/aerosonde.yaml Normal file
View File

@ -0,0 +1,33 @@
TRIM_E: 0
TRIM_A: 0
TRIM_R: 0
TRIM_T: 0.6
COURSE_KP: 0.7329
COURSE_KD: 0
COURSE_KI: 0.07
ROLL_KP: 1.17
ROLL_KD: -0.13
ROLL_KI: 0
PITCH_KP: 0.1
PITCH_KD: -0.017
PITCH_KI: 0
PITCH_FF: 0
AS_PITCH_KP: -0.0713
AS_PITCH_KD: -0.0635
AS_PITCH_KI: 0
AS_THR_KP: 3.2
AS_THR_KD: 0
AS_THR_KI: 1.0
ALT_KP: 0.045
ALT_KD: 0
ALT_KI: 0.01
BETA_KP: -0.1164
BETA_KD: 0
BETA_KI: -0.0037111

41
Flight_control/src/enemy_suv/suv_move/param/bixler.yaml Normal file
View File

@ -0,0 +1,41 @@
TRIM_E: 0
TRIM_A: 0
TRIM_R: 0
TRIM_T: 0.6
COURSE_KP: 0.7329
COURSE_KD: 0
COURSE_KI: 0.0
ROLL_KP: 1.2855
ROLL_KD: -0.1
ROLL_KI: 0
PITCH_KP: 1.0
PITCH_KD: -0.17
PITCH_KI: 0
PITCH_FF: 0
AS_PITCH_KP: -0.0713
AS_PITCH_KD: -0.0635
AS_PITCH_KI: 0
AS_THR_KP: 3.2
AS_THR_KD: 0
AS_THR_KI: 1.0
ALT_KP: 0.045
ALT_KD: 0
ALT_KI: 0.01
BETA_KP: -0.1164
BETA_KD: 0
BETA_KI: -0.0037111
CHI_INFTY: 1.0472
K_PATH: 0.025
K_ORBIT: 2.00

203
Flight_control/src/enemy_suv/suv_move/src/controller_base.cpp Normal file
View File

@ -0,0 +1,203 @@
#include "controller_base.h"
#include "controller_example.h"
namespace rosplane
{
controller_base::controller_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base::vehicle_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base::controller_commands_callback,
this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
func_ = boost::bind(&controller_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
internals_pub_ = nh_.advertise<rosplane_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base::actuator_controls_publish, this);
command_recieved_ = false;
}
void controller_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void controller_base::controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void controller_base::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
{
params_.trim_e = config.TRIM_E;
params_.trim_a = config.TRIM_A;
params_.trim_r = config.TRIM_R;
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.r_kp = config.ROLL_KP;
params_.r_kd = config.ROLL_KD;
params_.r_ki = config.ROLL_KI;
params_.p_kp = config.PITCH_KP;
params_.p_kd = config.PITCH_KD;
params_.p_ki = config.PITCH_KI;
params_.p_ff = config.PITCH_FF;
params_.a_p_kp = config.AS_PITCH_KP;
params_.a_p_kd = config.AS_PITCH_KD;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
params_.b_kp = config.BETA_KP;
params_.b_kd = config.BETA_KD;
params_.b_ki = config.BETA_KI;
}
void controller_base::convert_to_pwm(controller_base::output_s &output)
{
output.delta_e = output.delta_e*params_.pwm_rad_e;
output.delta_a = output.delta_a*params_.pwm_rad_a;
output.delta_r = output.delta_r*params_.pwm_rad_r;
}
void controller_base::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
input.h = -vehicle_state_.position[2];
input.va = vehicle_state_.Va;
input.phi = vehicle_state_.phi;
input.theta = vehicle_state_.theta;
input.chi = vehicle_state_.chi;
input.psi = vehicle_state_.psi;
input.p = vehicle_state_.p;
input.q = vehicle_state_.q;
input.r = vehicle_state_.r;
input.Va_c = controller_commands_.Va_c;
input.h_c = controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
input.phi_ff = controller_commands_.phi_ff;
input.Ts = 0.01f;
struct output_s output;
//ROS_INFO("%g",input.va);
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
if (command_recieved_ == true)
{
control(params_, input, output);
convert_to_pwm(output);
rosflight_msgs::Command actuators;
/* publish actuator controls */
actuators.ignore = 0;
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
actuators_pub_.publish(actuators);
//ROS_INFO("Hello-----------------------------------------------------------------------");
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
if (internals_pub_.getNumSubscribers() > 0)
{
rosplane_msgs::Controller_Internals inners;
inners.phi_c = output.phi_c;
inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_controller");
rosplane::controller_base *cont = new rosplane::controller_example();
ros::spin();
return 0;
}

203
Flight_control/src/enemy_suv/suv_move/src/controller_base99.cpp Normal file
View File

@ -0,0 +1,203 @@
#include "controller_base99.h"
#include "controller_example99.h"
namespace rosplane
{
controller_base99::controller_base99():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base99::vehicle_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base99::controller_commands_callback,
this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
func_ = boost::bind(&controller_base99::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
internals_pub_ = nh_.advertise<rosplane_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base99::actuator_controls_publish, this);
command_recieved_ = false;
}
void controller_base99::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void controller_base99::controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void controller_base99::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
{
params_.trim_e = config.TRIM_E;
params_.trim_a = config.TRIM_A;
params_.trim_r = config.TRIM_R;
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.r_kp = config.ROLL_KP;
params_.r_kd = config.ROLL_KD;
params_.r_ki = config.ROLL_KI;
params_.p_kp = config.PITCH_KP;
params_.p_kd = config.PITCH_KD;
params_.p_ki = config.PITCH_KI;
params_.p_ff = config.PITCH_FF;
params_.a_p_kp = config.AS_PITCH_KP;
params_.a_p_kd = config.AS_PITCH_KD;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
params_.b_kp = config.BETA_KP;
params_.b_kd = config.BETA_KD;
params_.b_ki = config.BETA_KI;
}
void controller_base99::convert_to_pwm(controller_base99::output_s &output)
{
output.delta_e = output.delta_e*params_.pwm_rad_e;
output.delta_a = output.delta_a*params_.pwm_rad_a;
output.delta_r = output.delta_r*params_.pwm_rad_r;
}
void controller_base99::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
input.h = -vehicle_state_.position[2];//>0
input.va = vehicle_state_.Va;
input.phi = vehicle_state_.phi;
input.theta = vehicle_state_.theta;
input.chi = vehicle_state_.chi;
input.psi = vehicle_state_.psi;
input.p = vehicle_state_.p;
input.q = vehicle_state_.q;
input.r = vehicle_state_.r;
input.Va_c = controller_commands_.Va_c;
input.h_c = controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
input.phi_ff = controller_commands_.phi_ff;
input.Ts = 0.01f;
struct output_s output;
//ROS_INFO("%g",input.va);
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
if (command_recieved_ == true)
{
control(params_, input, output);
convert_to_pwm(output);
rosflight_msgs::Command actuators;
/* publish actuator controls */
actuators.ignore = 0;
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
actuators_pub_.publish(actuators);
//ROS_INFO("Hello-----------------------------------------------------------------------");
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
if (internals_pub_.getNumSubscribers() > 0)
{
rosplane_msgs::Controller_Internals inners;
inners.phi_c = output.phi_c;
inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_controller99");
rosplane::controller_base99 *cont = new rosplane::controller_example99();
ros::spin();
return 0;
}

265
Flight_control/src/enemy_suv/suv_move/src/controller_example.cpp Normal file
View File

@ -0,0 +1,265 @@
#include "controller_example.h"
namespace rosplane
{
controller_example::controller_example() : controller_base()
{
current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator = 0;
p_error_ = 0;
p_integrator_ = 0;
}
void controller_example::control(const params_s &params, const input_s &input, output_s &output)
{
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
switch (current_zone)
{
case alt_zones::TAKE_OFF:
output.phi_c = 0;
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
output.delta_t = params.max_t;
output.theta_c = 15.0*3.14/180.0;
if (input.h >= params.alt_toz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::CLIMB:
output.delta_t = params.max_t;
//modified by kobe
if (input.Va_c < input.va)
{
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
}
else
{
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
}
//
if (input.h >= input.h_c - params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
else if (input.h <= params.alt_toz)
{
ROS_WARN("takeoff");
current_zone = alt_zones::TAKE_OFF;
}
break;
case alt_zones::DESCEND:
output.delta_t = 0;
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
if (input.h <= input.h_c + params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
break;
case alt_zones::ALTITUDE_HOLD:
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
if (input.h >= input.h_c + params.alt_hz)
{
ROS_WARN("desend");
current_zone = alt_zones::DESCEND;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
else if (input.h <= input.h_c - params.alt_hz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
default:
break;
}
output.current_zone = current_zone;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float controller_example::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params, float Ts)
{
float error = chi_c - chi;
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
float up = params.c_kp*error;
float ui = params.c_ki*c_integrator_;
float ud = params.c_kd*r;
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
if (fabs(params.c_ki) >= 0.00001)
{
float phi_c_unsat = up + ui + ud + phi_ff;
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
}
c_error_ = error;
return phi_c;
}
float controller_example::roll_hold(float phi_c, float phi, float p, const params_s &params, float Ts)
{
float error = phi_c - phi;
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
float up = params.r_kp*error;
float ui = params.r_ki*r_integrator;
float ud = params.r_kd*p;
float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
if (fabs(params.r_ki) >= 0.00001)
{
float delta_a_unsat = up + ui + ud;
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
}
r_error_ = error;
return delta_a;
}
float controller_example::pitch_hold(float theta_c, float theta, float q, const params_s &params, float Ts)
{
float error = theta_c - theta;
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
float up = params.p_kp*error;
float ui = params.p_ki*p_integrator_;
float ud = params.p_kd*q;
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
if (fabs(params.p_ki) >= 0.00001)
{
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
}
p_error_ = error;
return delta_e;
}
float controller_example::airspeed_with_pitch_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - ap_error_);
float up = params.a_p_kp*error;
float ui = params.a_p_ki*ap_integrator_;
float ud = params.a_p_kd*ap_differentiator_;
float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
if (fabs(params.a_p_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
}
ap_error_ = error;
return theta_c;
}
float controller_example::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - at_error_);
float up = params.a_t_kp*error;
float ui = params.a_t_ki*at_integrator_;
float ud = params.a_t_kd*at_differentiator_;
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
if (fabs(params.a_t_ki) >= 0.00001)
{
float delta_t_unsat = params.trim_t + up + ui + ud;
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
}
at_error_ = error;
return delta_t;
}
float controller_example::altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - a_error_);
float up = params.a_kp*error;
float ui = params.a_ki*a_integrator_;
float ud = params.a_kd*a_differentiator_;
float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
if (fabs(params.a_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
}
at_error_ = error;
return theta_c;
}
//float controller_example::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float controller_example::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} //end namespace

293
Flight_control/src/enemy_suv/suv_move/src/controller_example99.cpp Normal file
View File

@ -0,0 +1,293 @@
#include "controller_example99.h"
namespace rosplane
{
controller_example99::controller_example99() : controller_base99()
{
current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator = 0;
p_error_ = 0;
p_integrator_ = 0;
}
void controller_example99::control(const params_s &params, const input_s &input, output_s &output)
{
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
switch (current_zone)
{
case alt_zones::TAKE_OFF:
output.phi_c = 0;
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
output.delta_t = params.max_t;//modified by kobe
output.theta_c = 1.0*3.14/180.0;
//output.theta_c = 15.0*3.14/180.0;
//ROS_WARN("TAKE_OFF");
if (input.h >= params.alt_toz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::CLIMB:
output.delta_t = params.max_t;
//modified by kobe
if (input.Va_c < input.va)
{
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);//original
}
else
{
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
}
//
if (input.h >= input.h_c - params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
else if (input.h <= params.alt_toz)
{
//ROS_WARN("takeoff");
current_zone = alt_zones::TAKE_OFF;
}
break;
case alt_zones::DESCEND:
output.delta_t = decend_airspeed_oil(input.h_c, input.h, params, input.Ts);
//output.theta_c =airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c =decend_airspeed_with_pitch_hold(input.h_c, input.h, params, input.Ts);
//ROS_INFO("Look here %f %f",output.theta_c,pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts));
if (input.h <= input.h_c + params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
break;
case alt_zones::ALTITUDE_HOLD:
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
//ROS_INFO("Look here %f %f %f",output.delta_t,output.theta_c,input.Va_c);
if (input.h >= input.h_c + 200)//add by kobe
//if (input.h >= input.h_c + params.alt_hz)//original
{
ROS_WARN("desend");
current_zone = alt_zones::DESCEND;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
else if (input.h <= input.h_c - 200)
//else if (input.h <= input.h_c - params.alt_hz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
default:
break;
}
output.current_zone = current_zone;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float controller_example99::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params, float Ts)
{
float error = chi_c - chi;
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
float up = params.c_kp*error;
float ui = params.c_ki*c_integrator_;
float ud = params.c_kd*r;
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0); //add by kobe
//float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
if (fabs(params.c_ki) >= 0.00001)
{
float phi_c_unsat = up + ui + ud + phi_ff;
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
}
c_error_ = error;
return phi_c;
}
float controller_example99::roll_hold(float phi_c, float phi, float p, const params_s &params, float Ts)
{
float error = phi_c - phi;
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
float up = params.r_kp*error;
float ui = params.r_ki*r_integrator;
float ud = params.r_kd*p;
float delta_a = sat(up + ui + ud,params.max_a, -params.max_a);//add by kobe,0.1,-0.1
//float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
if (fabs(params.r_ki) >= 0.00001)
{
float delta_a_unsat = up + ui + ud;
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
}
r_error_ = error;
return delta_a;
}
float controller_example99::pitch_hold(float theta_c, float theta, float q, const params_s &params, float Ts)
{
float error = theta_c - theta;
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
float up = params.p_kp*error;
float ui = params.p_ki*p_integrator_;
float ud = params.p_kd*q;
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, 0.1, -0.1);// add by kobe
//float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
if (fabs(params.p_ki) >= 0.00001)
{
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
}
p_error_ = error;
return delta_e;
}
float controller_example99::airspeed_with_pitch_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - ap_error_);
float up = params.a_p_kp*error;
float ui = params.a_p_ki*ap_integrator_;
float ud = params.a_p_kd*ap_differentiator_;
float theta_c = sat(up + ui + ud, 2.0*3.14/180.0, -2.0*3.14/180.0);//add by kobe
//float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
if (fabs(params.a_p_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
}
ap_error_ = error;
return theta_c;
}
float controller_example99::decend_airspeed_with_pitch_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = 0.01*(h_c - h);
float theta_c = sat(error, 0, -90.0*3.14/180.0);
return theta_c;
}
float controller_example99::decend_airspeed_oil(float h_c, float h, const params_s &params, float Ts)
{
float error = pow(2,(h_c - h)/50);
float theta_t = sat(error, 1, 0);
return theta_t;
}
float controller_example99::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - at_error_);
float up = params.a_t_kp*error;
float ui = params.a_t_ki*at_integrator_;
float ud = params.a_t_kd*at_differentiator_;
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
if (fabs(params.a_t_ki) >= 0.00001)
{
float delta_t_unsat = params.trim_t + up + ui + ud;
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
}
at_error_ = error;
return delta_t;
}
float controller_example99::altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - a_error_);
float up = params.a_kp*error;
float ui = params.a_ki*a_integrator_;
float ud = params.a_kd*a_differentiator_;
float theta_c = sat(up + ui + ud, 5.0*3.14/180.0, -5.0*3.14/180.0);//add by kobe
//float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
if (fabs(params.a_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
}
at_error_ = error;
return theta_c;
}
//float controller_example99::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float controller_example99::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} //end namespace

228
Flight_control/src/enemy_suv/suv_move/src/estimator_base.cpp Normal file
View File

@ -0,0 +1,228 @@
#include "estimator_base.h"
#include "estimator_example.h"
namespace rosplane
{
estimator_base::estimator_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
nh_private_.param<std::string>("gps_topic", gps_topic_, "gps");
nh_private_.param<std::string>("imu_topic", imu_topic_, "imu/data");
nh_private_.param<std::string>("baro_topic", baro_topic_, "baro");
nh_private_.param<std::string>("airspeed_topic", airspeed_topic_, "airspeed");
nh_private_.param<std::string>("status_topic", status_topic_, "status");
nh_private_.param<double>("update_rate", update_rate_, 100.0);
params_.Ts = 1.0f/update_rate_;
params_.gravity = 9.8;
nh_private_.param<double>("rho", params_.rho, 1.225);
nh_private_.param<double>("sigma_accel", params_.sigma_accel, 0.0245);
nh_private_.param<double>("sigma_n_gps", params_.sigma_n_gps, 0.21);
nh_private_.param<double>("sigma_e_gps", params_.sigma_e_gps, 0.21);
nh_private_.param<double>("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500);
nh_private_.param<double>("sigma_couse_gps", params_.sigma_course_gps, 0.0045);
gps_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::gpsCallback, this);
imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this);
baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this);
airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this);
status_sub_ = nh_.subscribe(status_topic_, 1, &estimator_base::statusCallback, this);
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base::update, this);
vehicle_state_pub_ = nh_.advertise<rosplane_msgs::State>("state", 10);
init_static_ = 0;
baro_count_ = 0;
armed_first_time_ = false;
}
void estimator_base::update(const ros::TimerEvent &)
{
struct output_s output;
if (armed_first_time_)
{
estimate(params_, input_, output);
}
else
{
output.pn = output.pe = output.h = 0;
output.phi = output.theta = output.psi = 0;
output.alpha = output.beta = output.chi = 0;
output.p = output.q = output.r = 0;
output.Va = 0;
}
input_.gps_new = false;
rosplane_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
msg.position[0] = output.pn;
msg.position[1] = output.pe;
msg.position[2] = -output.h;
if (gps_init_)
{
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
}
msg.Va = output.Va;
msg.alpha = output.alpha;
msg.beta = output.beta;
msg.phi = output.phi;
msg.theta = output.theta;
msg.psi = output.psi;
msg.chi = output.chi;
msg.p = output.p;
msg.q = output.q;
msg.r = output.r;
msg.Vg = output.Vg;
msg.wn = output.wn;
msg.we = output.we;
msg.quat_valid = false;
msg.psi_deg = fmod(output.psi, 2*M_PI)*180/M_PI; //-360 to 360
msg.psi_deg += (msg.psi_deg < -180 ? 360 : 0);
msg.psi_deg -= (msg.psi_deg > 180 ? 360 : 0);
msg.chi_deg = fmod(output.chi, 2*M_PI)*180/M_PI; //-360 to 360
msg.chi_deg += (msg.chi_deg < -180 ? 360 : 0);
msg.chi_deg -= (msg.chi_deg > 180 ? 360 : 0);
vehicle_state_pub_.publish(msg);
}
void estimator_base::gpsCallback(const rosflight_msgs::GPS &msg)
{
if (msg.fix != true || msg.NumSat < 4 || !std::isfinite(msg.latitude))
{
input_.gps_new = false;
return;
}
if (!gps_init_)
{
gps_init_ = true;
init_alt_ = msg.altitude;
init_lat_ = msg.latitude;
init_lon_ = msg.longitude;
}
else
{
input_.gps_n = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
input_.gps_h = msg.altitude - init_alt_;
input_.gps_Vg = msg.speed;
if (msg.speed > 0.3)
input_.gps_course = msg.ground_course;
input_.gps_new = true;
//ROS_INFO("%g %g %g",input_.gps_n,input_.gps_e,input_.gps_h);
}
}
void estimator_base::imuCallback(const sensor_msgs::Imu &msg)
{
input_.accel_x = msg.linear_acceleration.x;
input_.accel_y = msg.linear_acceleration.y;
input_.accel_z = msg.linear_acceleration.z;
input_.gyro_x = msg.angular_velocity.x;
input_.gyro_y = msg.angular_velocity.y;
input_.gyro_z = msg.angular_velocity.z;
}
void estimator_base::baroAltCallback(const rosflight_msgs::Barometer &msg)
{
if (armed_first_time_ && !baro_init_)
{
if (baro_count_ < 100)
{
init_static_ += msg.pressure;
init_static_vector_.push_back(msg.pressure);
input_.static_pres = 0;
baro_count_ += 1;
}
else
{
init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(),
0.0)/init_static_vector_.size();
baro_init_ = true;
//Check that it got a good calibration.
std::sort(init_static_vector_.begin(), init_static_vector_.end());
float q1 = (init_static_vector_[24] + init_static_vector_[25])/2.0;
float q3 = (init_static_vector_[74] + init_static_vector_[75])/2.0;
float IQR = q3 - q1;
float upper_bound = q3 + 2.0*IQR;
float lower_bound = q1 - 2.0*IQR;
for (int i = 0; i < 100; i++)
{
if (init_static_vector_[i] > upper_bound)
{
baro_init_ = false;
baro_count_ = 0;
init_static_vector_.clear();
ROS_WARN("Bad baro calibration. Recalibrating");
break;
}
else if (init_static_vector_[i] < lower_bound)
{
baro_init_ = false;
baro_count_ = 0;
init_static_vector_.clear();
ROS_WARN("Bad baro calibration. Recalibrating");
break;
}
}
}
}
else
{
float static_pres_old = input_.static_pres;
input_.static_pres = -msg.pressure + init_static_;
float gate_gain = 1.35*params_.rho*params_.gravity;
if (input_.static_pres < static_pres_old - gate_gain)
{
input_.static_pres = static_pres_old - gate_gain;
}
else if (input_.static_pres > static_pres_old + gate_gain)
{
input_.static_pres = static_pres_old + gate_gain;
}
}
}
void estimator_base::airspeedCallback(const rosflight_msgs::Airspeed &msg)
{
float diff_pres_old = input_.diff_pres;
input_.diff_pres = msg.differential_pressure;
float gate_gain = pow(3, 2)*params_.rho/2.0;
if (input_.diff_pres < diff_pres_old - gate_gain)
{
input_.diff_pres = diff_pres_old - gate_gain;
}
else if (input_.diff_pres > diff_pres_old + gate_gain)
{
input_.diff_pres = diff_pres_old + gate_gain;
}
}
void estimator_base::statusCallback(const rosflight_msgs::Status &msg)
{
if (!armed_first_time_ && msg.armed)
armed_first_time_ = true;
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_estimator");
rosplane::estimator_base *est = new rosplane::estimator_example();
ros::spin();
return 0;
}

76
Flight_control/src/enemy_suv/suv_move/src/estimator_base_99pub.cpp Normal file
View File

@ -0,0 +1,76 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_99pub.h"
namespace rosplane
{
estimator_base_99pub::estimator_base_99pub():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
true_state_sub_ = nh_.subscribe("truth", 1, &estimator_base_99pub::truestateCallback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base_99pub::update, this);
vehicle_state99_pub_ = nh_.advertise<rosplane_msgs::State29>("state29", 10);
state_init_ = false;
}
void estimator_base_99pub::update(const ros::TimerEvent &)
{
rosplane_msgs::State29 msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position_gps[0]=(true_state_.position[0]*180)/(EARTH_RADIUS*M_PI)+true_state_.initial_lat;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position_gps[1]=(true_state_.position[1]*180)/(EARTH_RADIUS*cos(true_state_.initial_lat*M_PI/180.0)*M_PI)+true_state_.initial_lon;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
// input_.gps_h = msg.altitude - init_alt_;
msg.position_gps[2] =-true_state_.position[2]-true_state_.initial_alt; //>0
msg.attitude_angle[0] = true_state_.phi;
msg.attitude_angle[1] = true_state_.theta;
msg.attitude_angle[2] = true_state_.psi;
msg.velocity[0] = true_state_.Vn;
msg.velocity[1] = true_state_.Ve;
msg.velocity[2] = true_state_.Vd;
msg.angular_velocity[0] = true_state_.p;
msg.angular_velocity[1] = true_state_.q;
msg.angular_velocity[2] = true_state_.r;
msg.acceleration[0] = -1;
msg.acceleration[1] = -1;
msg.acceleration[2] = -1;
msg.electric_quantity = -1;
msg.state_word = -1;
// ROS_INFO("GPS-position: %g %g %g",msg.position_gps[0],msg.position_gps[1],msg.position_gps[2]);
// ROS_INFO("xyz-position: %g %g %g",true_state_.position[0],true_state_.position[1],true_state_.position[2]);
}
else
{
// ROS_WARN("No plane truth data.");
}
vehicle_state99_pub_.publish(msg);
}
void estimator_base_99pub::truestateCallback(const rosplane_msgs::StateConstPtr &msg)
{
true_state_ = *msg;
state_init_ = true;
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_estimator_29pub");
rosplane::estimator_base_99pub *cont = new rosplane::estimator_base_99pub();
ros::spin();
return 0;
}

75
Flight_control/src/enemy_suv/suv_move/src/estimator_base_statesub.cpp Normal file
View File

@ -0,0 +1,75 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_statesub.h"
namespace rosplane
{
estimator_base_statesub::estimator_base_statesub() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
state29_sub_ = nh_.subscribe("state29", 10, &estimator_base_statesub::state99Callback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_statesub::update, this);
vehicle_state_pub_ = nh_.advertise<rosplane_msgs::State>("state", 10);
state_init_ = false;
init_lat_=0; /**< Initial latitude in degrees */
init_lon_=0; /**< Initial longitude in degrees */
init_alt_=0; //>0
}
void estimator_base_statesub::update(const ros::TimerEvent &)
{
rosplane_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position[0] = EARTH_RADIUS*(state29_.position_gps[0] - init_lat_)*M_PI/180.0;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.position_gps[1] - init_lon_)*M_PI/180.0;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
msg.position[2] = -state29_.position_gps[2]- init_alt_; //<0
//input_.gps_h = msg.altitude - init_alt_;
msg.Wd=state29_.position_gps[0];
msg.Jd=state29_.position_gps[1];
msg.Va=sqrt(pow(state29_.velocity[0], 2.0) + pow(state29_.velocity[1], 2.0) + pow(state29_.velocity[2], 2.0));
msg.Vg=msg.Va; //true for rosflight
msg.phi=state29_.attitude_angle[0];
msg.theta=state29_.attitude_angle[1];
msg.psi=state29_.attitude_angle[2];
//msg.chi=msg.psi;//watch here
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
msg.p=state29_.angular_velocity[0];
msg.q=state29_.angular_velocity[1];
msg.r=state29_.angular_velocity[2];
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
}
else
{
// ROS_WARN("No State29 data.");
}
vehicle_state_pub_.publish(msg);
}
void estimator_base_statesub::state99Callback(const rosplane_msgs::State29ConstPtr &msg)
{
state29_ = *msg;
state_init_ = true;
}
} // namespace rosplane
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_estimator_statesub");
rosplane::estimator_base_statesub *cont = new rosplane::estimator_base_statesub();
ros::spin();
return 0;
}

76
Flight_control/src/enemy_suv/suv_move/src/estimator_base_updata_statesub.cpp Normal file
View File

@ -0,0 +1,76 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_updata_statesub.h"
namespace rosplane
{
estimator_base_updata_statesub::estimator_base_updata_statesub() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
state29_sub_ = nh_.subscribe("/serial_commu_up", 10, &estimator_base_updata_statesub::state99Callback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_updata_statesub::update, this);
vehicle_state_pub_ = nh_.advertise<rosplane_msgs::State>("state", 10);
state_init_ = false;
init_lat_=0; /**< Initial latitude in degrees */
init_lon_=0; /**< Initial longitude in degrees */
init_alt_=0; //>0
}
void estimator_base_updata_statesub::update(const ros::TimerEvent &)
{
rosplane_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position[0] = EARTH_RADIUS*(state29_.latitude - init_lat_)*M_PI/180.0;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.longitude - init_lon_)*M_PI/180.0;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
msg.position[2] = -state29_.altitude- init_alt_; //<0
//input_.gps_h = msg.altitude - init_alt_;
msg.Wd=state29_.latitude;
msg.Jd=state29_.longitude;
msg.Va=sqrt(pow(state29_.north_direction_speed, 2.0) + pow(state29_.east_direction_speed, 2.0) + pow(state29_.ground_direction_speed, 2.0));
msg.Vg=msg.Va; //true for rosflight
msg.phi=state29_.roll_angle;
msg.theta=state29_.pitch_angle;
msg.psi=state29_.yaw_angle;
//msg.chi=msg.psi;//watch here
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
msg.p=state29_.angular_rate_x;
msg.q=state29_.angular_rate_y;
msg.r=state29_.angular_rate_z;
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
// ROS_WARN("Recieved up-data data.");
//ROS_INFO("Hello--- %f %f %f %f",state29_.latitude,state29_.longitude,msg.Vg,state29_.altitude);
}
else
{
// ROS_WARN("No up-data data.");
}
vehicle_state_pub_.publish(msg);
}
void estimator_base_updata_statesub::state99Callback(const rosplane_msgs::Up_Data_NewConstPtr &msg)
{
state29_ = *msg;
state_init_ = true;
}
} // namespace rosplane
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_estimator_updata_statesub");
rosplane::estimator_base_updata_statesub *cont = new rosplane::estimator_base_updata_statesub();
ros::spin();
return 0;
}

416
Flight_control/src/enemy_suv/suv_move/src/estimator_example.cpp Normal file
View File

@ -0,0 +1,416 @@
#include "estimator_base.h"
#include "estimator_example.h"
namespace rosplane
{
float radians(float degrees)
{
return M_PI*degrees/180.0;
}
estimator_example::estimator_example() :
estimator_base(),
xhat_a_(Eigen::Vector2f::Zero()),
P_a_(Eigen::Matrix2f::Identity()),
Q_a_(Eigen::Matrix2f::Identity()),
xhat_p_(Eigen::VectorXf::Zero(7)),
P_p_(Eigen::MatrixXf::Identity(7, 7)),
Q_p_(Eigen::MatrixXf::Identity(7, 7)),
R_p_(Eigen::MatrixXf::Zero(7, 7)),
f_p_(7),
A_p_(7, 7),
C_p_(7),
L_p_(7)
{
P_a_ *= powf(radians(5.0f), 2);
Q_a_(0, 0) = 0.00000001;
Q_a_(1, 1) = 0.00000001;
P_p_ = Eigen::MatrixXf::Identity(7, 7);
P_p_(0, 0) = .03;
P_p_(1, 1) = .03;
P_p_(2, 2) = .01;
P_p_(3, 3) = radians(5.0f);
P_p_(4, 4) = .04;
P_p_(5, 5) = .04;
P_p_(6, 6) = radians(5.0f);
Q_p_ *= 0.0001f;
Q_p_(3, 3) = 0.000001f;
phat_ = 0;
qhat_ = 0;
rhat_ = 0;
phihat_ = 0;
thetahat_ = 0;
psihat_ = 0;
Vwhat_ = 0;
lpf_static_ = 0;
lpf_diff_ = 0;
N_ = 10;
alpha_ = 0.0f;
}
void estimator_example::estimate(const params_s &params, const input_s &input, output_s &output)
{
if (alpha_ == 0.0f) //initailze stuff that comes from params
{
R_accel_ = powf(params.sigma_accel, 2);
R_p_(0, 0) = powf(params.sigma_n_gps, 2);
R_p_(1, 1) = powf(params.sigma_e_gps, 2);
R_p_(2, 2) = powf(params.sigma_Vg_gps, 2);
R_p_(3, 3) = powf(params.sigma_course_gps, 2);
R_p_(4, 4) = 0.001;
R_p_(5, 5) = 0.001;
float lpf_a = 50.0;
float lpf_a1 = 8.0;
alpha_ = exp(-lpf_a*params.Ts);
alpha1_ = exp(-lpf_a1*params.Ts);
}
// low pass filter gyros to estimate angular rates
lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x;
lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y;
lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z;
float phat = lpf_gyro_x_;
float qhat = lpf_gyro_y_;
float rhat = lpf_gyro_z_;
// low pass filter static pressure sensor and invert to esimate altitude
lpf_static_ = alpha1_*lpf_static_ + (1 - alpha1_)*input.static_pres;
float hhat = lpf_static_/params.rho/params.gravity;
// low pass filter diff pressure sensor and invert to extimate Va
lpf_diff_ = alpha1_*lpf_diff_ + (1 - alpha1_)*input.diff_pres;
// when the plane isn't moving or moving slowly, the noise in the sensor
// will cause the differential pressure to go negative. This will catch
// those cases.
if (lpf_diff_ <= 0)
lpf_diff_ = 0.000001;
float Vahat = sqrt(2/params.rho*lpf_diff_);
// low pass filter accelerometers
lpf_accel_x_ = alpha_*lpf_accel_x_ + (1 - alpha_)*input.accel_x;
lpf_accel_y_ = alpha_*lpf_accel_y_ + (1 - alpha_)*input.accel_y;
lpf_accel_z_ = alpha_*lpf_accel_z_ + (1 - alpha_)*input.accel_z;
// implement continuous-discrete EKF to estimate roll and pitch angles
// prediction step
float cp; // cos(phi)
float sp; // sin(phi)
float tt; // tan(thata)
float ct; // cos(thata)
float st; // sin(theta)
for (int i = 0; i < N_; i++)
{
cp = cosf(xhat_a_(0)); // cos(phi)
sp = sinf(xhat_a_(0)); // sin(phi)
tt = tanf(xhat_a_(1)); // tan(thata)
ct = cosf(xhat_a_(1)); // cos(thata)
f_a_(0) = phat + (qhat*sp + rhat*cp)*tt;
f_a_(1) = qhat*cp - rhat*sp;
A_a_ = Eigen::Matrix2f::Zero();
A_a_(0, 0) = (qhat*cp - rhat*sp)*tt;
A_a_(0, 1) = (qhat*sp + rhat*cp)/ct/ct;
A_a_(1, 0) = -qhat*sp - rhat*cp;
xhat_a_ += f_a_*(params.Ts/N_);
P_a_ += (A_a_*P_a_ + P_a_*A_a_.transpose() + Q_a_)*(params.Ts/N_);
}
// measurement updates
cp = cosf(xhat_a_(0));
sp = sinf(xhat_a_(0));
ct = cosf(xhat_a_(1));
st = sinf(xhat_a_(1));
Eigen::Matrix2f I;
I = Eigen::Matrix2f::Identity();
// x-axis accelerometer
h_a_ = qhat*Vahat*st + params.gravity*st;
C_a_ = Eigen::Vector2f::Zero();
C_a_(1) = qhat*Vahat*ct + params.gravity*ct;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*((hhat < 15 ? lpf_accel_x_/3 : lpf_accel_x_) - h_a_);
// y-axis accelerometer
h_a_ = rhat*Vahat*ct - phat*Vahat*st - params.gravity*ct*sp;
C_a_ = Eigen::Vector2f::Zero();
C_a_(0) = -params.gravity*cp*ct;
C_a_(1) = -rhat*Vahat*st - phat*Vahat*ct + params.gravity*st*sp;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*(lpf_accel_y_ - h_a_);
// z-axis accelerometer
h_a_ = -qhat*Vahat*ct - params.gravity*ct*cp;
C_a_ = Eigen::Vector2f::Zero();
C_a_(0) = params.gravity*sp*ct;
C_a_(1) = (qhat*Vahat + params.gravity*cp)*st;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*(lpf_accel_z_ - h_a_);
check_xhat_a();
float phihat = xhat_a_(0);
float thetahat = xhat_a_(1);
// implement continous-discrete EKF to estimate pn, pe, chi, Vg
// prediction step
float psidot, tmp, Vgdot;
if (fabsf(xhat_p_(2)) < 0.01f)
{
xhat_p_(2) = 0.01; // prevent devide by zero
}
for (int i = 0; i < N_; i++)
{
psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat);
tmp = -psidot*Vahat*(xhat_p_(4)*cosf(xhat_p_(6)) + xhat_p_(5)*sinf(xhat_p_(6)))/xhat_p_(2);
Vgdot = ((Vahat*cosf(xhat_p_(6)) + xhat_p_(4))*(-psidot*Vahat*sinf(xhat_p_(6))) + (Vahat*sinf(xhat_p_(
6)) + xhat_p_(5))*(psidot*Vahat*cosf(xhat_p_(6))))/xhat_p_(2);
f_p_ = Eigen::VectorXf::Zero(7);
f_p_(0) = xhat_p_(2)*cosf(xhat_p_(3));
f_p_(1) = xhat_p_(2)*sinf(xhat_p_(3));
f_p_(2) = Vgdot;
f_p_(3) = params.gravity/xhat_p_(2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
f_p_(6) = psidot;
A_p_ = Eigen::MatrixXf::Zero(7, 7);
A_p_(0, 2) = cos(xhat_p_(3));
A_p_(0, 3) = -xhat_p_(2)*sinf(xhat_p_(3));
A_p_(1, 2) = sin(xhat_p_(3));
A_p_(1, 3) = xhat_p_(2)*cosf(xhat_p_(3));
A_p_(2, 2) = -Vgdot/xhat_p_(2);
A_p_(2, 4) = -psidot*Vahat*sinf(xhat_p_(6))/xhat_p_(2);
A_p_(2, 5) = psidot*Vahat*cosf(xhat_p_(6))/xhat_p_(2);
A_p_(2, 6) = tmp;
A_p_(3, 2) = -params.gravity/powf(xhat_p_(2), 2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
A_p_(3, 3) = -params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
A_p_(3, 6) = params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
xhat_p_ += f_p_*(params.Ts/N_);
P_p_ += (A_p_*P_p_ + P_p_*A_p_.transpose() + Q_p_)*(params.Ts/N_);
}
// while(xhat_p(3) > radians(180.0f)) xhat_p(3) = xhat_p(3) - radians(360.0f);
// while(xhat_p(3) < radians(-180.0f)) xhat_p(3) = xhat_p(3) + radians(360.0f);
// if(xhat_p(3) > radians(180.0f) || xhat_p(3) < radians(-180.0f))
// {
// ROS_WARN("Course estimate not wrapped from -pi to pi");
// xhat_p(3) = 0;
// }
// measurement updates
if (input.gps_new)
{
Eigen::MatrixXf I_p(7, 7);
I_p = Eigen::MatrixXf::Identity(7, 7);
// gps North position
h_p_ = xhat_p_(0);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(0) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(0, 0) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_n - h_p_);
// gps East position
h_p_ = xhat_p_(1);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(1) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(1, 1) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_e - h_p_);
// gps ground speed
h_p_ = xhat_p_(2);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(2) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(2, 2) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_Vg - h_p_);
// gps course
//wrap course measurement
float gps_course = fmodf(input.gps_course, radians(360.0f));
while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f);
h_p_ = xhat_p_(3);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(3) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(3, 3) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(gps_course - h_p_);
// // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi)
// h_p = Vahat*cosf(xhat_p(6)) + xhat_p(4) - xhat_p(2)*cosf(xhat_p(3)); // pseudo measurement
// C_p = Eigen::VectorXf::Zero(7);
// C_p(2) = -cos(xhat_p(3));
// C_p(3) = xhat_p(2)*sinf(xhat_p(3));
// C_p(4) = 1;
// C_p(6) = -Vahat*sinf(xhat_p(6));
// L_p = (P_p*C_p)/(R_p(4,4) + (C_p.transpose()*P_p*C_p));
// P_p = (I_p - L_p*C_p.transpose())*P_p;
// xhat_p = xhat_p + L_p*(0 - h_p);
// // pseudo measurement #2 y_2 = Va*sin(psi) + we - Vg*sin(chi)
// h_p = Vahat*sinf(xhat_p(6))+xhat_p(5)-xhat_p(2)*sinf(xhat_p(3)); // pseudo measurement
// C_p = Eigen::VectorXf::Zero(7);
// C_p(2) = -sin(xhat_p(3));
// C_p(3) = -xhat_p(2)*cosf(xhat_p(3));
// C_p(5) = 1;
// C_p(6) = Vahat*cosf(xhat_p(6));
// L_p = (P_p*C_p)/(R_p(5,5) + (C_p.transpose()*P_p*C_p));
// P_p = (I_p - L_p*C_p.transpose())*P_p;
// xhat_p = xhat_p + L_p*(0 - h_p);
if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000)
{
ROS_WARN("gps n limit reached");
xhat_p_(0) = input.gps_n;
}
if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000)
{
ROS_WARN("gps e limit reached");
xhat_p_(1) = input.gps_e;
}
}
bool problem = false;
int prob_index;
for (int i = 0; i < 7; i++)
{
if (!std::isfinite(xhat_p_(i)))
{
if (!problem)
{
problem = true;
prob_index = i;
}
switch (i)
{
case 0:
xhat_p_(i) = input.gps_n;
break;
case 1:
xhat_p_(i) = input.gps_e;
break;
case 2:
xhat_p_(i) = input.gps_Vg;
break;
case 3:
xhat_p_(i) = input.gps_course;
break;
case 6:
xhat_p_(i) = input.gps_course;
break;
default:
xhat_p_(i) = 0;
}
P_p_ = Eigen::MatrixXf::Identity(7, 7);
P_p_(0, 0) = .03;
P_p_(1, 1) = .03;
P_p_(2, 2) = .01;
P_p_(3, 3) = radians(5.0f);
P_p_(4, 4) = .04;
P_p_(5, 5) = .04;
P_p_(6, 6) = radians(5.0f);
}
}
if (problem)
{
ROS_WARN("position estimator reinitialized due to non-finite state %d", prob_index);
}
if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f))
{
//xhat_p(3) = fmodf(xhat_p(3),2*M_PI);
xhat_p_(6) = fmodf(xhat_p_(6), 2*M_PI);
}
float pnhat = xhat_p_(0);
float pehat = xhat_p_(1);
float Vghat = xhat_p_(2);
float chihat = xhat_p_(3);
float wnhat = xhat_p_(4);
float wehat = xhat_p_(5);
float psihat = xhat_p_(6);
output.pn = pnhat;
output.pe = pehat;
output.h = hhat;
output.Va = Vahat;
output.alpha = 0;
output.beta = 0;
output.phi = phihat;
output.theta = thetahat;
output.chi = chihat;
output.p = phat;
output.q = qhat;
output.r = rhat;
output.Vg = Vghat;
output.wn = wnhat;
output.we = wehat;
output.psi = psihat;
}
void estimator_example::check_xhat_a()
{
if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0)))
{
if (!std::isfinite(xhat_a_(0)))
{
xhat_a_(0) = 0;
P_a_ = Eigen::Matrix2f::Identity();
P_a_ *= powf(radians(20.0f), 2);
ROS_WARN("attiude estimator reinitialized due to non-finite roll");
}
else if (xhat_a_(0) > radians(85.0))
{
xhat_a_(0) = radians(82.0);
//ROS_WARN("max roll angle");
}
else if (xhat_a_(0) < radians(-85.0))
{
xhat_a_(0) = radians(-82.0);
//ROS_WARN("min roll angle");
}
}
if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1)))
{
if (!std::isfinite(xhat_a_(1)))
{
xhat_a_(1) = 0;
P_a_ = Eigen::Matrix2f::Identity();
P_a_ *= powf(radians(20.0f), 2);
ROS_WARN("attiude estimator reinitialized due to non-finite pitch");
}
else if (xhat_a_(1) > radians(80.0))
{
xhat_a_(1) = radians(77.0);
ROS_WARN("max pitch angle");
}
else if (xhat_a_(1) < radians(-80.0))
{
xhat_a_(1) = radians(-77.0);
ROS_WARN("min pitch angle");
}
}
}
} //end namespace

162
Flight_control/src/enemy_suv/suv_move/src/path_follower_base.cpp Normal file
View File

@ -0,0 +1,162 @@
#include "path_follower_base.h"
#include "path_follower_example.h"
namespace rosplane
{
path_follower_base::path_follower_base() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
vehicle_state_sub_ = nh_.subscribe<rosplane_msgs::State>("state", 1, &path_follower_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("ground_truth/state", 1, &path_follower_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("/test/odom", 1, &path_follower_base::bebop1_state_callback, this);
current_path_sub_ = nh_.subscribe<rosplane_msgs::Current_Path>("current_path", 1,
&path_follower_base::current_path_callback, this);
nh_private_.param<double>("CHI_INFTY", params_.chi_infty, 1.0472);
nh_private_.param<double>("K_PATH", params_.k_path, 0.025);
nh_private_.param<double>("K_ORBIT", params_.k_orbit, 4.0);
func_ = boost::bind(&path_follower_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_follower_base::update, this);
controller_commands_pub_ = nh_.advertise<rosplane_msgs::Controller_Commands>("controller_commands", 1);
state_init_ = false;
current_path_init_ = false;
}
void path_follower_base::update(const ros::TimerEvent &)
{
struct output_s output;
if (state_init_ == true && current_path_init_ == true)
{
follow(params_, input_, output);
rosplane_msgs::Controller_Commands msg;
msg.chi_c = output.chi_c;
msg.landing=input_.landing;
msg.takeoff=input_.takeoff;
if (input_.Va_c_path == 0)
{
// ROS_INFO("Hello1");
msg.Va_c = 0;
}
else
{
// ROS_INFO("Hello2");
msg.Va_c = input_.Va_d;
}
// ROS_INFO("what %f",msg.Va_c);
msg.h_c = output.h_c;
msg.phi_ff = output.phi_ff;
controller_commands_pub_.publish(msg);
}
}
void path_follower_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg)
{
// input_.pn = msg->position[0]; /** position north */
// input_.pe = msg->position[1]; /** position east */
// input_.h = -msg->position[2]; /** altitude */
// input_.chi = msg->chi;
// input_.Va = msg->Va;
// state_init_ = true;
}
void path_follower_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
// ROS_INFO("%g", msg->pose.pose.position.x); /** position north */
input_.pn = msg->pose.pose.position.x; /** position north */
input_.pe = msg->pose.pose.position.y; /** position east */
input_.h = msg->pose.pose.position.z; /** altitude */
input_.Va = sqrt(pow(msg->twist.twist.linear.x, 2) + pow(msg->twist.twist.linear.y, 2));
input_.chi = path_follower_base::Quaternion_to_Euler(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
// ROS_INFO("%g %g %g %g %g",input_.pn,input_.pe,input_.h,input_.Va,input_.chi);
state_init_ = true;
}
float path_follower_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
{
float Radx;
float sum;
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
if (sum == 0)
{
sum = 0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
if (fabs(q2) < fabs(q3))
{
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
if (q2 * q3 > 0)
{
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
}
//Radx = asin(2.0f*(q2*q3 + q0*q1));
//Rady = atan2(-2 * q1 * q3 + 2 * q0 * q2, q3*q3 - q2 * q2 - q1 * q1 + q0 * q0);
//Radz = atan2(2 * q1*q2 - 2 * q0*q3, q2*q2 - q3*q3 + q0*q0 - q1*q1);
//return Radx;
}
void path_follower_base::current_path_callback(const rosplane_msgs::Current_PathConstPtr &msg)
{
if (msg->path_type == msg->LINE_PATH)
input_.p_type = path_type::Line;
else if (msg->path_type == msg->ORBIT_PATH)
input_.p_type = path_type::Orbit;
else if (msg->path_type == msg->STAR_PATH) // add by kobe
input_.p_type = path_type::Star;
input_.Va_d = msg->Va_d;
// ROS_INFO("hello %f",input_.Va_d);
for (int i = 0; i < 3; i++)
{
input_.r_path[i] = msg->r[i];
input_.q_path[i] = msg->q[i];
input_.c_orbit[i] = msg->c[i];
}
input_.rho_orbit = msg->rho;
input_.lam_orbit = msg->lambda;
input_.h_c_path = msg->h_c; //add by kobe
input_.Va_c_path = msg->Va_d;
input_.landing = msg->landing;
input_.takeoff = msg->takeoff;
current_path_init_ = true;
}
void path_follower_base::reconfigure_callback(rosplane::FollowerConfig &config, uint32_t level)
{
params_.chi_infty = config.CHI_INFTY;
params_.k_path = config.K_PATH;
params_.k_orbit = config.K_ORBIT;
}
} // namespace rosplane
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_path_follower");
rosplane::path_follower_base *path = new rosplane::path_follower_example();
ros::spin();
return 0;
}

81
Flight_control/src/enemy_suv/suv_move/src/path_follower_example.cpp Normal file
View File

@ -0,0 +1,81 @@
#include "path_follower_example.h"
namespace rosplane
{
path_follower_example::path_follower_example()
{
}
void path_follower_example::follow(const params_s &params, const input_s &input, output_s &output)
{
if (input.p_type == path_type::Line) // follow straight line path specified by r and q
{
// compute wrapped version of the path angle
float chi_q = atan2f(input.q_path[1], input.q_path[0]);
// modified by kobe
float chi=input.chi;
while (chi < -M_PI)
chi += 2*M_PI;
while (chi > M_PI)
chi -= 2*M_PI;
while (chi_q - chi < -M_PI)
chi_q += 2*M_PI;
while (chi_q - chi > M_PI)
chi_q -= 2*M_PI;
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
// heading command
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error);
//ROS_INFO("hello1: %g %g %g %g",chi_q,input.chi,chi,output.chi_c);
// desired altitude
output.h_c = -input.h_c_path;//h_c_path is negative;h_c is positive.
//kobe modified end
/* ////for truth not state
while (chi_q - input.chi < -M_PI)
chi_q += 2*M_PI;
while (chi_q - input.chi > M_PI)
chi_q -= 2*M_PI;
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
// heading command
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error); //desired altitude
float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe),
2))*(input.q_path[2])/sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2));
// commanded altitude is desired altitude
output.h_c = h_d;// -input.r_path[2] - x// important for hover// kobe denote
*/
output.phi_ff = 0.0;
}
else if(input.p_type == path_type::Orbit)// follow a orbit path specified by c_orbit, rho_orbit, and lam_orbit
{
float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]),
2)); // distance from orbit center
// compute wrapped version of angular position on orbit
float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]);
while ((varphi - input.chi) < -M_PI)
varphi += 2*M_PI;
while ((varphi - input.chi) > M_PI)
varphi -= 2*M_PI;
//compute orbit error
float norm_orbit_error = (d - input.rho_orbit)/input.rho_orbit;
output.chi_c = varphi + input.lam_orbit*(M_PI/2 + atanf(params.k_orbit*norm_orbit_error));
// commanded altitude is the height of the orbit
float h_d = -input.c_orbit[2];
output.h_c = h_d;
output.phi_ff = 0;//(norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0);
}
else if(input.p_type == path_type::Star)// towards to a point in any direction on platform,r is the center;add by kobe
{
}
output.Va_c = input.Va_d;
}
} //end namespace

238
Flight_control/src/enemy_suv/suv_move/src/path_manager_base.cpp Normal file
View File

@ -0,0 +1,238 @@
#include "path_manager_base.h"
#include "path_manager_example.h"
namespace rosplane
{
path_manager_base::path_manager_base() : nh_(ros::NodeHandle()), /** nh_ stuff added here */
nh_private_(ros::NodeHandle("~"))
{
nh_private_.param<double>("R_min", params_.R_min, 25); //25
nh_private_.param<double>("update_rate", update_rate_, 10.0);
vehicle_state_sub_ = nh_.subscribe("state", 10, &path_manager_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &path_manager_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &path_manager_base::bebop1_state_callback, this);
new_waypoint_sub_ = nh_.subscribe("waypoint_path", 10, &path_manager_base::new_waypoint_callback, this);
current_path_pub_ = nh_.advertise<rosplane_msgs::Current_Path>("current_path", 10);
goal_info_pub_ = nh_.advertise<rosplane_msgs::Goal_Info>("Goal_Info", 10);
down_data_pub_ = nh_.advertise<rosplane_msgs::Down_Data_New>("serial_commu_down", 10);
waypoint_received_sub_ = nh_.subscribe("new_waypoint", 10, &path_manager_base::waypoint_received_callback, this);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_manager_base::current_path_publish, this);
num_waypoints_ = 0;
state_init_ = false;
waypoint_received_ = false;
}
void path_manager_base::waypoint_received_callback(const rosplane_msgs::Waypoint &msg)
{
float ell = sqrtf((waypoint_end_.w[0] - msg.w[0]) * (waypoint_end_.w[0] - msg.w[0]) +
(waypoint_end_.w[1] - msg.w[1]) * (waypoint_end_.w[1] - msg.w[1]));
if (ell < 2.0 * params_.R_min)
{
ROS_ERROR("The distance between nodes must be larger than 2R.");
return;
}
waypoint_start_ = waypoint_end_;
waypoint_end_.w[0] = msg.w[0];
waypoint_end_.w[1] = msg.w[1];
waypoint_end_.w[2] = msg.w[2];
waypoint_end_.chi_d = msg.chi_d;
waypoint_end_.chi_valid = msg.chi_valid;
waypoint_end_.Va_d = msg.Va_d;
waypoint_received_ = true;
}
void path_manager_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
//state_init_ = true;
}
void path_manager_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
bebop1_state_ = *msg;
state_init_ = true;
}
void path_manager_base::new_waypoint_callback(const rosplane_msgs::Waypoint &msg)
{
if (msg.clear_wp_list == true)
{
waypoints_.clear();
num_waypoints_ = 0;
idx_a_ = 0;
min_distance_ = 1000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
min_distance_titude = 10000000000;
//return;
}
if (msg.set_current || num_waypoints_ == 0)//mean to set the start point as the a goal-never use---kobe denote
{
waypoint_s currentwp;
currentwp.w[0] = bebop1_state_.pose.pose.position.x;
currentwp.w[1] = bebop1_state_.pose.pose.position.y;
currentwp.w[2] = (-bebop1_state_.pose.pose.position.z > -25 ? msg.w[2] : -bebop1_state_.pose.pose.position.z);
currentwp.chi_d= path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
// currentwp.w[0] = vehicle_state_.position[0];
// currentwp.w[1] = vehicle_state_.position[1];
// currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]);
// currentwp.chi_d = vehicle_state_.chi;
// add by kobe
currentwp.lat = vehicle_state_.Wd;
currentwp.lon = vehicle_state_.Jd;
currentwp.chi_valid = msg.chi_valid;
currentwp.Va_d = msg.Va_d;
currentwp.landing=msg.landing;
currentwp.takeoff=msg.takeoff;
waypoints_.clear();
waypoints_.push_back(currentwp);
num_waypoints_ = 1;
idx_a_ = 0;
min_distance_ = 10000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
min_distance_titude = 10000000000;
show = 1;
show2=1;
}
waypoint_s nextwp;
nextwp.w[0] = msg.w[0];
nextwp.w[1] = msg.w[1];
nextwp.w[2] = msg.w[2];
// add by kobe
nextwp.lat=msg.lat;
nextwp.lon=msg.lon;
nextwp.chi_d = msg.chi_d;
nextwp.chi_valid = msg.chi_valid;
nextwp.Va_d = msg.Va_d;
nextwp.landing=msg.landing;
nextwp.takeoff=msg.takeoff;
waypoints_.push_back(nextwp);
num_waypoints_++;
// ROS_INFO("num_waypoints_: %d", num_waypoints_);
}
void path_manager_base::current_path_publish(const ros::TimerEvent &)
{
struct input_s input;
input.pn = bebop1_state_.pose.pose.position.x; /** position north */
input.pe = bebop1_state_.pose.pose.position.y; /** position east */
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
input.chi =path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
// input.pn = vehicle_state_.position[0]; /** position north */
// input.pe = vehicle_state_.position[1]; /** position east */
// input.h = -vehicle_state_.position[2]; /** altitude */
// input.chi = vehicle_state_.chi;
// input.va = vehicle_state_.Va;
//add by kobe
input.lat=vehicle_state_.Wd;
input.lon=vehicle_state_.Jd;
struct output_s output;
if (state_init_ == true)
{
manage(params_, input, output);
}
rosplane_msgs::Current_Path current_path;
rosplane_msgs::Goal_Info goal_info;
rosplane_msgs::Down_Data_New down_data;
// modified by kobe
if (output.flag == 1)
current_path.path_type = current_path.LINE_PATH;
else if (output.flag == 0)
current_path.path_type = current_path.ORBIT_PATH;
else if (output.flag == 2)
current_path.path_type = current_path.STAR_PATH;
for (int i = 0; i < 3; i++)
{
current_path.r[i] = output.r[i];
current_path.q[i] = output.q[i];
current_path.c[i] = output.c[i];
}
current_path.rho = output.rho;
current_path.lambda = output.lambda;
current_path.h_c = output.h_c;
current_path.Va_d = output.Va_c; //new speed
current_path.takeoff=output.takeoff;
current_path.landing=output.landing;
current_path_pub_.publish(current_path);
goal_info.altitude=-output.h_c;
goal_info.v_d=output.Va_d;
goal_info.action=1;//desired action move
for (int i = 0; i < 2; i++)
{
goal_info.xy[i] = output.gxy[i];
goal_info.ll[i] = output.gll[i];
}
goal_info_pub_.publish(goal_info); // add by kobe
down_data.latitude = output.gll[0];
down_data.longitude = output.gll[1];
down_data.altitude = -output.h_c;
down_data.plane_speed= output.Va_d;
down_data.status_word= 1;
//ROS_INFO("Look here %f %f %f %f",down_data.latitude,down_data.longitude,down_data.altitude,output.Va_d);
down_data_pub_.publish(down_data); // add by kobe
}
//add by kobe
float path_manager_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
{
float Radx;
float sum;
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
if (sum == 0)
{
sum = 0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
if (fabs(q2) < fabs(q3))
{
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
if (q2 * q3 > 0)
{
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
}
}
} // namespace rosplane
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_path_manager");
rosplane::path_manager_base *est = new rosplane::path_manager_example();
ros::spin();
return 0;
}

717
Flight_control/src/enemy_suv/suv_move/src/path_manager_example.cpp Normal file
View File

@ -0,0 +1,717 @@
#include "path_manager_example.h"
#include "ros/ros.h"
#include <cmath>
namespace rosplane
{
path_manager_example::path_manager_example() : path_manager_base()
{
fil_state_ = fillet_state::STRAIGHT;
// use this !
dub_state_ = dubin_state::FIRST;
}
// done
void path_manager_example::manage(const params_s &params, const input_s &input, output_s &output)
{
//ROS_INFO("num_waypoints_: %d",num_waypoints_);
if (num_waypoints_ < 2)
{
ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m");
output.flag = 0; //modified by kobe
output.Va_d = 12;
output.c[0] = 0.0f;
output.c[1] = 0.0f;
output.c[2] = -50.0f;
// Radius of orbital path (m)
output.rho = params.R_min;
// Direction of orbital path (clockwise is 1, counterclockwise is -1)
output.lambda = 1;
}
else
{
// do this, idx_a_ is initialized zero
if (waypoints_[idx_a_].chi_valid)
{
manage_dubins(params, input, output);
}
else
{
/** Switch the following for flying directly to waypoints, or filleting corners */
// manage_line(params, input, output);
//manage_fillet(params, input, output);
manage_star(params, input, output); //add by kobe
}
}
}
// add by kobe
void path_manager_example::manage_star(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f w_start;
w_start << input.pn, input.pe, -input.h; //aircraft's position
int idx_b;
int idx_c;
float the_distance_;
double the_distance_titude; //for latitude and longtitude
idx_b = idx_a_ + 1;
if (idx_a_ < num_waypoints_ - 2)
{
idx_c = idx_b + 1;
}
else
{
idx_c = idx_b;
}
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w); //the goal
the_distance_ = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2));
the_distance_titude = earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon);
output.flag = 1; //modified by kobe
output.Va_d = waypoints_[idx_b].Va_d; //modified by kobe
//ROS_INFO("position: %g %g %g",w_start(0),w_start(1),w_start(2));
//ROS_INFO("Current info are: %f %f %f %f", input.va, -input.h,w_start(0),w_start(1));
output.r[0] = w_start(0);
output.r[1] = w_start(1);
output.r[2] = w_start(2);
Eigen::Vector3f q_im1 = (w_i - w_start).normalized();
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
output.gxy[0] = w_i(0);
output.gxy[1] = w_i(1);
output.gll[0] = waypoints_[idx_b].lat;
output.gll[1] = waypoints_[idx_b].lon;
output.h_c = w_i(2); //altitude of the goal
if (idx_a_ == 0&&show2 == 1)
{
ROS_INFO("First goal info are: %f %f", w_i(0), w_i(1));
ROS_INFO("Desired fly speed and height are: %f %f", output.Va_d, -w_i(2));
show2=0;
}
//enter the slow-down area,recompute the speed
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < output.Va_d * 2 && idx_a_ == num_waypoints_ - 2) //enter the slowdown-ball and the goal is the last goal
{
output.Va_c = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2))/2;
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5 )
{
output.Va_c = 0;
}
// ROS_INFO("Now the speed is: %f",output.Va_c);
}
else
{
output.Va_c = output.Va_d;
}
output.landing=waypoints_[idx_b].landing;
output.takeoff=waypoints_[idx_b].takeoff;
/* //1.enter a tiny ball and switch goal
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 5) //enter a tiny ball and switch goal
{
//ROS_INFO("00000o- %g %g %g", w_start(2), w_i(2), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
//ROS_INFO("last goal is: %f %f", w_i(0), w_i(1));
//ROS_INFO("Va- %f/n", input.va);
if (idx_a_ == num_waypoints_ - 2)
{
idx_a_ = idx_a_; //do not update goal
//ROS_INFO("idx_a_- %f/n", idx_a_);
}
else
{
idx_a_++;
}
}
*/
/*
//2.enter a half-plane and switch,ahead 5 m from the goal point
Eigen::Vector3f w_i_plane;
w_i_plane=w_i;
w_i_plane(0)=w_i_plane(0)-5*( ( w_i(0)-w_im1(0) ) / ( 0.001+fabs(w_i(0)-w_im1(0)) ) );
w_i_plane(1)=w_i_plane(1)-5*( ( w_i(1)-w_im1(1) ) / ( 0.001+fabs(w_i(1)-w_im1(1)) ) );
if ((w_i_plane - w_start).normalized().dot((w_i_plane - w_im1).normalized())<0)//plane-method
{
if (idx_a_ == num_waypoints_ - 2)
idx_a_ = idx_a_;//do not update goal
else
idx_a_++;
}
*/
//3.Enter a large ball(large-enter-ball) first. Anyway, when the distance from the aircraft to goal become larger,switch to the next goal.
if (the_distance_ < min_distance_)
{
min_distance_ = the_distance_;
}
if (the_distance_titude < min_distance_titude)
{
min_distance_titude = the_distance_titude;
}
// ROS_INFO("distance is : %.20f %f", earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
//ROS_INFO("000distance- %f %f", the_distance_, min_distance_);
//1.进入大圈且最小距离开始增大时;2.进入一个小圈时<E59C88>?分别针对北东坐标和经纬度坐标
judge_condition1 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 3 && the_distance_ > min_distance_ + 1);
judge_condition2 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 3 && the_distance_titude > min_distance_titude + 1);
judge_condition3 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5);
judge_condition4 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 0.5);
// height judge
judge_condition5 = (sqrt(pow(w_start(2) - w_i(2), 2)) < 10);
//notice here
judge_condition2=0;
judge_condition4=0;
if ((judge_condition1 || judge_condition2 || judge_condition3 || judge_condition4) && judge_condition5) //switch
{
//ROS_INFO("distance- %f %f", the_distance_, min_distance_);
if (idx_a_ == num_waypoints_ - 2)
{
if (show == 1)
{
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
ROS_INFO("No more goal, just hover and wait for the next goal.");
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
ROS_INFO("Current speed and height are: %f %f\n\n", input.va, input.h);
show = 0;
}
idx_a_ = idx_a_; //do not update goal
}
else
{
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
ROS_INFO("Current speed and height are: %f %f", input.va, input.h);
ROS_INFO("New goal info are: %f %f\n\n", w_ip1(0), w_ip1(1));
min_distance_ = sqrt(pow(w_i(0) - w_ip1(0), 2) + pow(w_i(1) - w_ip1(1), 2));
idx_a_++;
}
}
//
}
double path_manager_example::earth_distance(double latitude1, double longitude1, double latitude2, double longitude2)
{
double Lat1 = latitude1 * M_PI_F / 180.00; // 纬度
double Lat2 = latitude2 * M_PI_F / 180.00;
double a = Lat1 - Lat2; //两点纬度之差
double b = longitude1 * M_PI_F / 180.00 - longitude2 * M_PI_F / 180.00; //经度之差
double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(Lat1) * cos(Lat2) * pow(sin(b / 2), 2))); //计算两点距离的公<E79A84>?/better when value is very samll
//double s2 = acos(cos(Lat1)*cos(Lat2)*cos(b)+sin(Lat1)*sin(Lat2));//计算两点距离的公<E79A84>?
s = s * EARTH_RADIUS; //弧长乘地球半径（半径为米<E4B8BA>?
//s2 = s2 * EARTH_RADIUS;//弧长乘地球半径（半径为米<E4B8BA>?
return s;
}
void path_manager_example::manage_line(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f p;
p << input.pn, input.pe, -input.h;
int idx_b;
int idx_c;
if (idx_a_ == num_waypoints_ - 1)
{
idx_b = 0;
idx_c = 1;
}
else if (idx_a_ == num_waypoints_ - 2)
{
idx_b = num_waypoints_ - 1;
idx_c = 0;
}
else
{
idx_b = idx_a_ + 1;
idx_c = idx_b + 1;
}
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w);
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
output.flag = true;
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = w_im1(0);
output.r[1] = w_im1(1);
output.r[2] = w_im1(2);
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
Eigen::Vector3f n_i = (q_im1 + q_i).normalized();
if ((p - w_i).dot(n_i) > 0.0f)
{
if (idx_a_ == num_waypoints_ - 1)
idx_a_ = 0;
else
idx_a_++;
}
}
void path_manager_example::manage_fillet(const params_s &params, const input_s &input, output_s &output)
{
if (num_waypoints_ < 3) //since it fillets don't make sense between just two points
{
manage_line(params, input, output);
return;
}
Eigen::Vector3f p;
p << input.pn, input.pe, -input.h;
int idx_b;
int idx_c;
if (idx_a_ == num_waypoints_ - 1)
{
idx_b = 0;
idx_c = 1;
}
else if (idx_a_ == num_waypoints_ - 2)
{
idx_b = num_waypoints_ - 1;
idx_c = 0;
}
else
{
idx_b = idx_a_ + 1;
idx_c = idx_b + 1;
}
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w);
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
float R_min = params.R_min;
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = w_im1(0);
output.r[1] = w_im1(1);
output.r[2] = w_im1(2);
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
float beta = acosf(-q_im1.dot(q_i));
Eigen::Vector3f z;
switch (fil_state_)
{
case fillet_state::STRAIGHT:
output.flag = 1; //modified by kobe
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
output.c[0] = 1;
output.c[1] = 1;
output.c[2] = 1;
output.rho = 1;
output.lambda = 1;
z = w_i - q_im1 * (R_min / tanf(beta / 2.0));
if ((p - z).dot(q_im1) > 0)
fil_state_ = fillet_state::ORBIT;
break;
case fillet_state::ORBIT:
output.flag = 0; //modified by kobe
output.q[0] = q_i(0);
output.q[1] = q_i(1);
output.q[2] = q_i(2);
Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(beta / 2.0));
output.c[0] = c(0);
output.c[1] = c(1);
output.c[2] = c(2);
output.rho = R_min;
output.lambda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1);
z = w_i + q_i * (R_min / tanf(beta / 2.0));
if ((p - z).dot(q_i) > 0)
{
if (idx_a_ == num_waypoints_ - 1)
idx_a_ = 0;
else
idx_a_++;
fil_state_ = fillet_state::STRAIGHT;
}
break;
}
}
void path_manager_example::manage_dubins(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f p;
//ROS_INFO("input.pn: %f, input.pe: %f, -input.h: %f",input.pn, input.pe, -input.h);
p << input.pn, input.pe, -input.h;
/* uint8 path_type # Indicates strait line or orbital path
float32 Va_d # Desired airspeed (m/s)
float32[3] r # Vector to origin of straight line path (m)
float32[3] q # Unit vector, desired direction of travel for line path
float32[3] c # Center of orbital path (m)
float32 rho # Radius of orbital path (m)
int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1)
uint8 ORBIT_PATH = 0
uint8 LINE_PATH = 1
int8 CLOCKWISE = 1
int8 COUNT_CLOCKWISE = -1 */
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = 0;
output.r[1] = 0;
output.r[2] = 0;
output.q[0] = 0;
output.q[1] = 0;
output.q[2] = 0;
output.c[0] = 0;
output.c[1] = 0;
output.c[2] = 0;
//ROS_INFO("dub_state_: %d",dub_state_);
switch (dub_state_)
{
case dubin_state::FIRST:
// path planning
// Algorithm11 P211
dubinsParameters(waypoints_[0], waypoints_[1], params.R_min);
waypoint_start_ = waypoints_[0];
waypoint_end_ = waypoints_[1];
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
// p: current pos
// w1: waypoint 1
// q1: unit vector of H1
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
break;
case dubin_state::BEFORE_H1:
//waypoint_received_ = false;
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1
{
dub_state_ = dubin_state::STRAIGHT;
}
break;
case dubin_state::BEFORE_H1_WRONG_SIDE:
//waypoint_received_ = false;
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1
{
dub_state_ = dubin_state::BEFORE_H1;
}
break;
case dubin_state::STRAIGHT:
output.flag = 1; //modified by kobe
output.r[0] = dubinspath_.w1(0);
output.r[1] = dubinspath_.w1(1);
output.r[2] = dubinspath_.w1(2);
// output.r[0] = dubinspath_.z1(0);
// output.r[1] = dubinspath_.z1(1);
// output.r[2] = dubinspath_.z1(2);
output.q[0] = dubinspath_.q1(0);
output.q[1] = dubinspath_.q1(1);
output.q[2] = dubinspath_.q1(2);
output.rho = 1;
output.lambda = 1;
//ROS_INFO("pos: [%f,%f,%f],w2: [%f,%f,%f],q1: [%f,%f,%f]",p[0],p[1],p[2],dubinspath_.w2[0],dubinspath_.w2[1],dubinspath_.w2[2],dubinspath_.q1[0],dubinspath_.q1[1],dubinspath_.q1[2]);
//ROS_INFO("w3: [%f,%f,%f]",dubinspath_.w3[0],dubinspath_.w3[1],dubinspath_.w3[2]);
//ROS_INFO("w1: [%f,%f,%f]",dubinspath_.w1[0],dubinspath_.w1[1],dubinspath_.w1[2]);
if (waypoint_received_)
{
//start new path
// plan new Dubin's path to next waypoint configuration
waypoint_start_.w[0] = p[0];
waypoint_start_.w[1] = p[1];
waypoint_start_.w[2] = p[2];
waypoint_start_.chi_d = 0;
waypoint_start_.chi_valid = true;
waypoint_start_.Va_d = 12;
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
waypoint_received_ = false;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
}
else
{
if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2
{
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3
{
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H3;
}
}
}
break;
case dubin_state::BEFORE_H3:
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.ce(0);
output.c[1] = dubinspath_.ce(1);
output.c[2] = dubinspath_.ce(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lame;
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3
{
if (!waypoint_received_)
{
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
//dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
}
else
{
//dub_state_ = dubin_state::BEFORE_H1;
dub_state_ = dubin_state::BEFORE_H3;
}
}
else
{
//start new path
// plan new Dubin's path to next waypoint configuration
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
waypoint_received_ = false;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
}
}
break;
case dubin_state::BEFORE_H3_WRONG_SIDE:
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.ce(0);
output.c[1] = dubinspath_.ce(1);
output.c[2] = dubinspath_.ce(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lame;
/*
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
{
if(!waypoint_received_)
//dub_state_ = dubin_state::BEFORE_H1;
dub_state_ = dubin_state::BEFORE_H3;
else
dub_state_ = dubin_state::BEFORE_H1;
}
*/
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
dub_state_ = dubin_state::BEFORE_H3;
else
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
break;
}
}
Eigen::Matrix3f path_manager_example::rotz(float theta)
{
Eigen::Matrix3f R;
R << cosf(theta), -sinf(theta), 0,
sinf(theta), cosf(theta), 0,
0, 0, 1;
return R;
}
// done
float path_manager_example::mo(float in)
{
float val;
if (in > 0)
val = fmod(in, 2.0 * M_PI_F);
else
{
float n = floorf(in / 2.0 / M_PI_F);
val = in - n * 2.0 * M_PI_F;
}
return val;
}
void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, float R)
{
float ell = sqrtf((start_node.w[0] - end_node.w[0]) * (start_node.w[0] - end_node.w[0]) +
(start_node.w[1] - end_node.w[1]) * (start_node.w[1] - end_node.w[1]));
if (ell < 2.0 * R)
{
ROS_ERROR("SHOULD NOT HAPPEN: The distance between nodes must be larger than 2R.");
}
else
{
dubinspath_.ps(0) = start_node.w[0];
dubinspath_.ps(1) = start_node.w[1];
dubinspath_.ps(2) = start_node.w[2];
dubinspath_.chis = start_node.chi_d;
dubinspath_.pe(0) = end_node.w[0];
dubinspath_.pe(1) = end_node.w[1];
dubinspath_.pe(2) = end_node.w[2];
dubinspath_.chie = end_node.chi_d;
Eigen::Vector3f crs = dubinspath_.ps;
crs(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chis) - sinf(M_PI_2_F) * sinf(dubinspath_.chis));
crs(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chis) + cosf(M_PI_2_F) * sinf(dubinspath_.chis));
Eigen::Vector3f cls = dubinspath_.ps;
cls(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chis) - sinf(-M_PI_2_F) * sinf(dubinspath_.chis));
cls(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chis) + cosf(-M_PI_2_F) * sinf(dubinspath_.chis));
Eigen::Vector3f cre = dubinspath_.pe;
cre(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chie) - sinf(M_PI_2_F) * sinf(dubinspath_.chie));
cre(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chie) + cosf(M_PI_2_F) * sinf(dubinspath_.chie));
Eigen::Vector3f cle = dubinspath_.pe;
cle(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chie) - sinf(-M_PI_2_F) * sinf(dubinspath_.chie));
cle(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chie) + cosf(-M_PI_2_F) * sinf(dubinspath_.chie));
float theta, theta2;
// compute L1
theta = atan2f(cre(1) - crs(1), cre(0) - crs(0));
float L1 = (crs - cre).norm() + R * mo(2.0 * M_PI_F + mo(theta - M_PI_2_F) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F));
// compute L2
ell = (cle - crs).norm();
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
float L2;
if (2.0 * R > ell)
L2 = 9999.0f;
else
{
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
L2 = sqrtf(ell * ell - 4.0 * R * R) + R * mo(2.0 * M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F));
}
// compute L3
ell = (cre - cls).norm();
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
float L3;
if (2.0 * R > ell)
L3 = 9999.0f;
else
{
theta2 = acosf(2.0 * R / ell);
L3 = sqrtf(ell * ell - 4 * R * R) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F));
}
// compute L4
theta = atan2f(cle(1) - cls(1), cle(0) - cls(0));
float L4 = (cls - cle).norm() + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F));
// L is the minimum distance
int idx = 1;
dubinspath_.L = L1;
if (L2 < dubinspath_.L)
{
dubinspath_.L = L2;
idx = 2;
}
if (L3 < dubinspath_.L)
{
dubinspath_.L = L3;
idx = 3;
}
if (L4 < dubinspath_.L)
{
dubinspath_.L = L4;
idx = 4;
}
Eigen::Vector3f e1;
// e1.zero();
e1(0) = 1;
e1(1) = 0;
e1(2) = 0;
switch (idx)
{
case 1:
dubinspath_.cs = crs;
dubinspath_.lams = 1;
dubinspath_.ce = cre;
dubinspath_.lame = 1;
dubinspath_.q1 = (cre - crs).normalized();
dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
break;
case 2:
dubinspath_.cs = crs;
dubinspath_.lams = 1;
dubinspath_.ce = cle;
dubinspath_.lame = -1;
ell = (cle - crs).norm();
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
dubinspath_.q1 = rotz(theta2 + M_PI_2_F) * e1;
dubinspath_.w1 = dubinspath_.cs + (rotz(theta2) * e1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F) * e1) * R;
break;
case 3:
dubinspath_.cs = cls;
dubinspath_.lams = -1;
dubinspath_.ce = cre;
dubinspath_.lame = 1;
ell = (cre - cls).norm();
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
theta2 = acosf(2.0 * R / ell);
dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F) * e1;
dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2) * e1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F) * e1) * R;
break;
case 4:
dubinspath_.cs = cls;
dubinspath_.lams = -1;
dubinspath_.ce = cle;
dubinspath_.lame = -1;
dubinspath_.q1 = (cle - cls).normalized();
dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
break;
}
dubinspath_.w3 = dubinspath_.pe;
dubinspath_.q3 = rotz(dubinspath_.chie) * e1;
dubinspath_.R = R;
}
}
} // namespace rosplane

62
Flight_control/src/enemy_suv/suv_move/src/path_planner.cpp Normal file
View File

@ -0,0 +1,62 @@
#include <ros/ros.h>
#include <rosplane_msgs/Waypoint.h>
#define EARTH_RADIUS 6378145.0f
#define num_waypoints 3
float pn2latitude(float pn,float init_lat_)
{
return (pn*180)/(EARTH_RADIUS*M_PI)+init_lat_;
}
float pe2longtitude(float pe,float init_lat_,float init_lon_)
{
return (pe*180)/(EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*M_PI)+init_lon_;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_simple_path_planner");
ros::NodeHandle nh_;
ros::Publisher waypointPublisher = nh_.advertise<rosplane_msgs::Waypoint>("waypoint_path", 10);
float Va = 3;
int tmp;
tmp = 7; //notice here
float wps[tmp * num_waypoints] =
{
// pn,pe,altitude,-,va,lat,lon
200, 100, 0, 0, 10, pn2latitude(200,0), pe2longtitude(100,0,0),
300, 200, 0, 0, 10, pn2latitude(300,0), pe2longtitude(200,0,0),
0, 200, 0, 0, 10, pn2latitude(0,0), pe2longtitude(200,0,0),
};
for (int i(0); i < num_waypoints; i++)
{
ros::Duration(0.5).sleep();
rosplane_msgs::Waypoint new_waypoint;
new_waypoint.w[0] = wps[i * tmp + 0];
new_waypoint.w[1] = wps[i * tmp + 1];
new_waypoint.w[2] = wps[i * tmp + 2];
new_waypoint.chi_d = wps[i * tmp + 3];
new_waypoint.Va_d = wps[i * tmp + 4];
// add by kobe
new_waypoint.lat = wps[i * tmp + 5];
new_waypoint.lon = wps[i * tmp + 6];
new_waypoint.chi_valid = false; //kobe
if (i == 0)
new_waypoint.set_current = true;
else
new_waypoint.set_current = false;
new_waypoint.clear_wp_list = false;
waypointPublisher.publish(new_waypoint);
}
ros::Duration(1.5).sleep();
return 0;
}

214
Flight_control/src/enemy_suv/suv_move/src/pseudo_controller_base.cpp Normal file
View File

@ -0,0 +1,214 @@
#include "pseudo_controller_base.h"
#include "pseudo_controller_example.h"
// modified by kobe
namespace rosplane
{
pseudo_controller_base::pseudo_controller_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
//ROS_INFO("hello");
vehicle_state_sub_ = nh_.subscribe("state", 10, &pseudo_controller_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &pseudo_controller_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &pseudo_controller_base::bebop1_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &pseudo_controller_base::controller_commands_callback,this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&bebop1_state_, 0, sizeof(bebop1_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.3);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 1.5);
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 0.2);
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
//nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 0.9);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.0);
//nh_private_.param<double>("MAX_A", params_.max_a, 0.523);//rotate_trans
//nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
nh_private_.param<double>("MAX_T", params_.max_t, 200.0);
func_ = boost::bind(&pseudo_controller_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
//pesudors_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
pesudors_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
internals_pub_ = nh_.advertise<rosplane_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &pseudo_controller_base::actuator_controls_publish, this);
command_recieved_ = false;
}
void pseudo_controller_base::vehicle_state_callback(const rosplane_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void pseudo_controller_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
bebop1_state_ = *msg;
}
void pseudo_controller_base::controller_commands_callback(const rosplane_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void pseudo_controller_base::reconfigure_callback(rosplane::ControllerConfig &config, uint32_t level)
{
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
//params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
}
void pseudo_controller_base::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
//input.h = -vehicle_state_.position[2];
//input.va = vehicle_state_.Va;
//input.chi = vehicle_state_.chi;
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
input.chi =pseudo_controller_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
// ROS_INFO("LOOK chi: %f",input.chi);
input.Va_c = controller_commands_.Va_c;
// ROS_INFO("LOOK Va_c: %f",input.Va_c);
input.h_c = controller_commands_.h_c;//3>controller_commands_.h_c)?3:controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
// ROS_INFO("LOOK chi_c: %f",input.chi_c);
input.takeoff = controller_commands_.takeoff;
input.landing = controller_commands_.landing;
input.Ts = 0.01f;
if (controller_commands_.Va_c==0)
{
reached=1;
}
struct output_s output;
if (command_recieved_ == true)
{
control(params_, input, output);
//convert_to_pwm(output);
geometry_msgs::Twist cmd_vel;
//rosflight_msgs::Command pseudors;
/* publish pseudors controls */
cmd_vel.linear.x = output.forward_trans;//head vel
cmd_vel.linear.y = output.left_right_trans;
// cmd_vel.linear.z = output.up_down_trans;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;//output.roll_trans;
if (input.va > 0.1)
cmd_vel.angular.z = output.rotate_trans;//output.rotate_trans
else
cmd_vel.angular.z = 0;
pesudors_pub_.publish(cmd_vel);
//ROS_INFO("hello %f",bebop1_state_.pose.pose.position.z);
// ROS_INFO("x-y-z-w: %f %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.linear.z,cmd_vel.angular.z);
// ROS_INFO("six: %f %f %f %f %f %f\n",input.h,input.h_c,input.va,input.Va_c,input.chi,input.chi_c);
// ROS_INFO("position: %f %f %f",bebop1_state_.pose.pose.position.x,bebop1_state_.pose.pose.position.y,bebop1_state_.pose.pose.position.z);
if (internals_pub_.getNumSubscribers() > 0)
{
rosplane_msgs::Controller_Internals inners;
//sinners.phi_c = output.phi_c;
//inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
float pseudo_controller_base::Quaternion_to_Euler(float q0,float q1,float q2,float q3)
{
float Radx;
float sum;
sum = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if (sum==0)
{
sum=0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
// ROS_INFO("LOOK here2: %f",asin(2.0f*(q2*q3 + q0*q1)));
if (fabs(q2)<fabs(q3))
{
Radx = asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
else
{
if(q2*q3>0)
{
Radx = 3.14159-asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
else
{
Radx = -3.14159-asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosplane_controller");
rosplane::pseudo_controller_base *cont = new rosplane::pseudo_controller_example();
ros::spin();
return 0;
}

326
Flight_control/src/enemy_suv/suv_move/src/pseudo_controller_example.cpp Normal file
View File

@ -0,0 +1,326 @@
#include "pseudo_controller_example.h"
//modified by kobe
namespace rosplane
{
pseudo_controller_example::pseudo_controller_example() : pseudo_controller_base()
{
current_zone = alt_zones::PREPARE;
//current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
at_error_ = 0;
at_integrator_ = 0;
ap_error_ = 0;
ap_integrator_ = 0;
}
void pseudo_controller_example::control(const params_s &params, const input_s &input, output_s &output)
{
output.up_down_trans = 0;
output.forward_trans = input.Va_c; // ZS
output.rotate_trans = 0;
output.left_right_trans = 0;
output.rotate_value=pseudo_course(input.chi, input.chi_c);
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// if (input.landing==true) // protect-landing first
// {
// current_zone = alt_zones::LANDING;
// }
// if (input.takeoff==true && current_zone==alt_zones::LANDING)
// {
// current_zone = alt_zones::PREPARE;
// }
// switch (current_zone)
// {
// case alt_zones::PREPARE:
// output.up_down_trans = 0.5;
// output.forward_trans = 0;
// output.left_right_trans = 0;
// output.rotate_trans=0;
// if (input.h >= 1) //
// {
// ROS_DEBUG("TAKEOFF");
// current_zone = alt_zones::TAKE_OFF;
// }
// break;
// case alt_zones::TAKE_OFF:
// //output.rotate_trans = 1.5*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);//recover
// //output.forward_trans = 0.7;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// output.forward_trans = input.Va_c;//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// output.up_down_trans = pow(1.1,input.h)*pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// //output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// //if (input.h >= params.alt_toz) //alt_toz=1.5//recovery
// //output.left_right_trans = 1.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// if (input.h >= 1.5) //
// {
// ROS_DEBUG("climb");
// current_zone = alt_zones::CLIMB;
// ap_error_ = 0;
// ap_integrator_ = 0;
// ap_differentiator_ = 0;
// }
// break;
// case alt_zones::CLIMB:
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// //output.forward_trans = 0.8;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// //output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// //output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// //output.rotate_trans = 0.3*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// ////if (input.h >= input.h_c - params.alt_hz) //alt_hz=0.2//recovery
// if (input.h >= input.h_c - 0.3) //alt_hz=0.2
// {
// ROS_DEBUG("hold");
// current_zone = alt_zones::ALTITUDE_HOLD;
// at_error_ = 0;
// at_integrator_ = 0;
// at_differentiator_ = 0;
// a_error_ = 0;
// a_integrator_ = 0;
// a_differentiator_ = 0;
// }
// //else if (input.h <= params.alt_toz) //1.5//recovery
// // else if (input.h <= 1) //1.5
// // {
// // ROS_DEBUG("takeoff");
// // current_zone = alt_zones::TAKE_OFF; // Is that necessary?
// // }
// break;
// case alt_zones::DESCEND:
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// //output.forward_trans = 0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// //output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// //output.rotate_trans = pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// //output.up_down_trans = 0.1 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
// //if (input.h <= input.h_c + params.alt_hz)//recovery
// if (input.h <= input.h_c + 0.3)
// {
// if (input.h < 0) //modify later
// {
// ROS_DEBUG("landing");
// current_zone = alt_zones::LANDING;
// // output.forward_trans = sat(input.Va_c*cos(output.rotate_trans),1,0);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts) * 0.5;
// // output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts) / 2;
// // output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// }
// else
// {
// ROS_DEBUG("hold");
// current_zone = alt_zones::ALTITUDE_HOLD;
// at_error_ = 0;
// at_integrator_ = 0;
// at_differentiator_ = 0;
// a_error_ = 0;
// a_integrator_ = 0;
// a_differentiator_ = 0;
// }
// }
// break;
// case alt_zones::ALTITUDE_HOLD:
// output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//0.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// output.up_down_trans = pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// //output.forward_trans = 1;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
// //output.left_right_trans = pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
// //output.up_down_trans = 0.1 * pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
// //output.rotate_trans = 0.7*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
// //if (input.h >= input.h_c + params.alt_hz)//recovery
// if (input.h >= input.h_c + 0.3)
// {
// ROS_DEBUG("desend");
// current_zone = alt_zones::DESCEND;
// ap_error_ = 0;
// ap_integrator_ = 0;
// ap_differentiator_ = 0;
// }
// //else if (input.h <= input.h_c - params.alt_hz)//recovery
// else if (input.h <= input.h_c - 0.3)
// {
// ROS_DEBUG("climb");
// current_zone = alt_zones::CLIMB;
// ap_error_ = 0;
// ap_integrator_ = 0;
// ap_differentiator_ = 0;
// }
// break;
// case alt_zones::LANDING:
// ROS_DEBUG("landing");
// current_zone = alt_zones::LANDING;
// ap_error_ = 0;
// ap_integrator_ = 0;
// ap_differentiator_ = 0;
// output.forward_trans = 0;
// if (input.h < 0.6)
// {
// output.up_down_trans = 0;
// }
// else
// {
// output.up_down_trans = -input.h/3;
// }
// output.rotate_trans =0;
// output.left_right_trans = 0;
// break;
// default:
// break;
// }
output.current_zone = current_zone;
//output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
//output.up_down_trans=pseudo_pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float pseudo_controller_example::pseudo_course(float chi, float chi_c)
{
chi_c=sat(chi_c,3.14159,-3.14159);
chi=sat(chi,3.14159,-3.14159);
float rotate = sat(chi_c-chi,2*3.14159,-2*3.14159);//-pi to pi
// float lr;
// if (rotate>3.14159)
// {
// lr=-1;
// }
// else if(rotate<-3.14159)
// {
// lr=1;
// }
//rotate=rotate+lr*2*3.14159;//more easier to rotate
// rotate=sat(rotate,3.14159/4,-3.14159/4);//limit
return rotate;
}
float pseudo_controller_example::pseudo_left_right(float chi, float chi_c, float forward_trans)
{
float error= chi_c-chi;
float a = fabs(forward_trans);
float x = (3.14 / 4 > fabs(error)) ? fabs(error) : 3.14 / 4;
float b = tan(x);
float c = error / (fabs(error + 0.001));
float left_right_trans = sat(a * b * c, forward_trans, -forward_trans);
return left_right_trans;
}
float pseudo_controller_example::pseudo_course_hold(float chi, float chi_c, const params_s &params, float Ts)
{
float error = chi_c-chi;
//c_integrator_ = c_integrator_ + (Ts / 2.0) * (error + c_error_);
//float up = params.c_kp * error;
//float ui = params.c_ki * c_integrator_;
float rotate_trans = sat((error + c_error_) / 2, 45.0 * 3.14 / 180.0, -45.0 * 3.14 / 180.0);
//if (fabs(params.c_ki) >= 0.00001)
//{
//float rotate_trans_unsat = up + ui;
//c_integrator_ = c_integrator_ + (Ts / params.c_ki) * (rotate_trans - rotate_trans_unsat);
//}
c_error_ = error;
return rotate_trans;
}
float pseudo_controller_example::pseudo_pitch_hold(float h_c, float h, const struct params_s &params, float Ts)
{
float error = h_c - h;
//ap_integrator_ = ap_integrator_ + (Ts / 2.0) * (error + ap_error_);
// float up = 0.5 * error;
//float ui = params.a_p_ki * ap_integrator_;
//float up_down_trans = sat(up + ui, 0.8, -0.8);//recovery
float up_down_trans = sat(0.5 * error, 2, -2);
//if (fabs(params.a_p_ki) >= 0.00001)
//{
//float up_down_trans_unsat = up + ui;
//ap_integrator_ = ap_integrator_ + (Ts / params.a_p_ki) * (up_down_trans - up_down_trans_unsat);
//}
// ap_error_ = error;
return up_down_trans;
}
float pseudo_controller_example::pseudo_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
//at_integrator_ = at_integrator_ + (Ts / 2.0) * (error + at_error_);
//at_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * at_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) *(error - at_error_);
float up = params.a_t_kp * error;
//float ui = params.a_t_ki * at_integrator_;
//float ud = params.a_t_kd * at_differentiator_;
//float forward_trans = sat(Va + up + ui + ud, params.max_t, 0.1);
float forward_trans = sat(Va+error, params.max_t, 0.5);
//if (fabs(params.a_t_ki) >= 0.00001)
//{
//float forward_trans_unsat = params.trim_t + up + ui + ud;
//at_integrator_ = at_integrator_ + (Ts / params.a_t_ki) * (forward_trans - forward_trans_unsat);
//}
//at_error_ = error;
return forward_trans;
}
float pseudo_controller_example::pseudo_altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
//a_integrator_ = a_integrator_ + (Ts / 2.0) * (error + a_error_);
//a_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * a_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) * (error - a_error_);
float up = 0.7 * error;
//float up = params.a_kp * error;
//ROS_INFO("hello %f",params.a_kp);
//float ui = params.a_ki * a_integrator_;
//float ud = params.a_kd * a_differentiator_;
//float up_down_trans = sat(up + ui + ud, 0.8, -0.8);//recovery
float up_down_trans = sat(up, 2, -2);
//if (fabs(params.a_ki) >= 0.00001)
//{
//float up_down_trans_unsat = up + ui + ud;
//a_integrator_ = a_integrator_ + (Ts / params.a_ki) * (up_down_trans - up_down_trans_unsat);
//}
// at_error_ = error;
return up_down_trans;
}
//float pseudo_controller_example::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float pseudo_controller_example::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} // namespace rosplane

1
Flight_control/src/hector_quadrotor Submodule

@ -0,0 +1 @@
Subproject commit c5accd2403622ddc275e576691b9831bd6e2694f

1
Flight_control/src/rosflight_ws Submodule

@ -0,0 +1 @@
Subproject commit 9404d28c2d96f498a5d7f6f14a639f9318c30d64