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

reset gazebo to origin

This commit is contained in:
XunMeng2017 2019-03-25 10:43:04 +08:00
parent 443393476a
commit 062acc406e
4948 changed files with 0 additions and 1271573 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Flight_control/.catkin_workspace
View File

@ -1 +0,0 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

1
Flight_control/src/CMakeLists.txt
View File

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

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

@ -1,47 +0,0 @@
################################################################################
# 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
View File

@ -1,41 +0,0 @@
<?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
View File

@ -1,40 +0,0 @@
<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
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 736 KiB

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

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 859 KiB

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

@ -1,30 +0,0 @@
# 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
View File

File diff suppressed because it is too large Load Diff

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

@ -1,14 +0,0 @@
<?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
View File

@ -1,138 +0,0 @@
<?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
View File

@ -1,229 +0,0 @@
<?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
View File

@ -1,71 +0,0 @@
<?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
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 14 MiB

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

@ -1,20 +0,0 @@
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
View File

@ -1,84 +0,0 @@
<?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
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
View File

@ -1,78 +0,0 @@
<?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
View File

@ -1,29 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
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
View File

@ -1,89 +0,0 @@
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
View File

@ -1,25 +0,0 @@
<?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
View File

@ -1,19 +0,0 @@
<?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
View File

@ -1,33 +0,0 @@
<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
View File

@ -1,215 +0,0 @@
//=================================================================================================
// 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
View File

@ -1,89 +0,0 @@
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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,7 +0,0 @@
<?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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,12 +0,0 @@
<?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
View File

@ -1,27 +0,0 @@
<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
View File

@ -1,382 +0,0 @@
//=================================================================================================
// 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
View File

@ -1,417 +0,0 @@
//=================================================================================================
// 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
View File

@ -1,123 +0,0 @@
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
View File

@ -1,58 +0,0 @@
#!/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
View File

@ -1,17 +0,0 @@
#!/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
View File

@ -1,145 +0,0 @@
/**
* @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
View File

@ -1,145 +0,0 @@
/**
* @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
View File

@ -1,53 +0,0 @@
#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
View File

@ -1,56 +0,0 @@
#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
View File

@ -1,131 +0,0 @@
/**
* @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
View File

@ -1,45 +0,0 @@
// 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
View File

@ -1,46 +0,0 @@
// 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
View File

@ -1,46 +0,0 @@
// 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
View File

@ -1,73 +0,0 @@
#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
View File

@ -1,103 +0,0 @@
//* @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
View File

@ -1,18 +0,0 @@
#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
View File

@ -1,122 +0,0 @@
//* @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
View File

@ -1,78 +0,0 @@
#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
View File

@ -1,137 +0,0 @@
/**
*
* 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
View File

@ -1,66 +0,0 @@
#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
View File

@ -1,33 +0,0 @@
<?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
View File

@ -1,33 +0,0 @@
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
View File

@ -1,41 +0,0 @@
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
View File

@ -1,203 +0,0 @@
#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
View File

@ -1,203 +0,0 @@
#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
View File

@ -1,265 +0,0 @@
#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
View File

@ -1,293 +0,0 @@
#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
View File

@ -1,228 +0,0 @@
#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
View File

@ -1,76 +0,0 @@
//* @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
View File

@ -1,75 +0,0 @@
//* @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
View File

@ -1,76 +0,0 @@
//* @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
View File

@ -1,416 +0,0 @@
#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
View File

@ -1,162 +0,0 @@
#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
View File

@ -1,81 +0,0 @@
#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
View File

@ -1,238 +0,0 @@
#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
View File

@ -1,717 +0,0 @@
#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
View File

@ -1,62 +0,0 @@
#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
View File

@ -1,214 +0,0 @@
#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
View File

@ -1,326 +0,0 @@
#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

4
Flight_control/src/hector_quadrotor/hector_quadrotor.rosinstall
View File

@ -1,4 +0,0 @@
- git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel}
- git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin}
- git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel}
- git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel}

26
Flight_control/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst
View File

@ -1,26 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
0.3.3 (2014-09-01)
------------------
* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo
* Contributors: Johannes Meyer
0.3.2 (2014-03-30)
------------------
* added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package
* Contributors: Johannes Meyer
0.3.1 (2013-12-26)
------------------
0.3.0 (2013-09-11)
------------------
* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack

4
Flight_control/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt
View File

@ -1,4 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor)
find_package(catkin REQUIRED)
catkin_metapackage()

30
Flight_control/src/hector_quadrotor/hector_quadrotor/package.xml
View File

@ -1,30 +0,0 @@
<package>
<name>hector_quadrotor</name>
<version>0.3.5</version>
<description>hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor</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>
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<!-- Dependencies needed after this package is compiled. -->
<run_depend>hector_quadrotor_controller</run_depend>
<run_depend>hector_quadrotor_description</run_depend>
<run_depend>hector_quadrotor_model</run_depend>
<run_depend>hector_quadrotor_teleop</run_depend>
<run_depend>hector_uav_msgs</run_depend>
<!-- Dependencies needed only for running tests. -->
</package>

46
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst
View File

@ -1,46 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
* updated angular/z controller parameters
* Remove redundant callback queue in twist_controller, use root_nh queue as per ros_control API
* Add controller timeout to allow faster shutdown of spawner
* Contributors: Johannes Meyer, Paul Bovbel
0.3.4 (2015-02-22)
------------------
* improved automatic landing detection and shutdown on rollovers
* slightly updated velocity controller limits and gains
* Contributors: Johannes Meyer
0.3.3 (2014-09-01)
------------------
* fixed some compiler warnings and missing return values
* increased integral gain for attitude stabilization (fix #12)
* make a copy of the root NodeHandle in all controllers
For some reason deconstructing the TwistController resulted in a pure virtual function call without this patch.
* Contributors: Johannes Meyer
0.3.2 (2014-03-30)
------------------
* Fix boost 1.53 issues
changed boost::shared_dynamic_cast to boost::dynamic_pointer_cast and
boost::shared_static_cast to boost::static_pointer_cast
* use a separate callback queue thread for the TwistController
* added optional twist limit in pose controller
* Contributors: Christopher Hrabia, Johannes Meyer
0.3.1 (2013-12-26)
------------------
* New controller implementation using ros_control
* Added pose controller
* Added motor controller that controls motor voltages from wrench commands
* 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
* Propulsion and aerodynamics models are in hector_quadrotor_model package now.
* Gazebo plugins are in hector_quadrotor_gazebo_plugins package now.

78
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt
View File

@ -1,78 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor_controller)
## 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 sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface)
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 include
LIBRARIES hector_quadrotor_controller
CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface
DEPENDS
)
###########
## Build ##
###########
add_library(hector_quadrotor_controller src/quadrotor_interface.cpp src/pid.cpp)
target_link_libraries(hector_quadrotor_controller ${catkin_LIBRARIES})
add_dependencies(hector_quadrotor_controller hector_uav_msgs_generate_messages_cpp)
add_library(hector_quadrotor_pose_controller src/pose_controller.cpp)
target_link_libraries(hector_quadrotor_pose_controller hector_quadrotor_controller)
add_library(hector_quadrotor_twist_controller src/twist_controller.cpp)
target_link_libraries(hector_quadrotor_twist_controller hector_quadrotor_controller ${BOOST_LIBRARIES})
add_library(hector_quadrotor_motor_controller src/motor_controller.cpp)
target_link_libraries(hector_quadrotor_motor_controller hector_quadrotor_controller)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executables and/or libraries for installation
install(TARGETS hector_quadrotor_controller hector_quadrotor_pose_controller hector_quadrotor_twist_controller hector_quadrotor_motor_controller
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"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY launch params DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(FILES
plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

536
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h
View File

@ -1,536 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
#define HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Wrench.h>
#include <sensor_msgs/Imu.h>
#include <hector_uav_msgs/MotorStatus.h>
#include <hector_uav_msgs/MotorCommand.h>
#include <hector_uav_msgs/AttitudeCommand.h>
#include <hector_uav_msgs/YawrateCommand.h>
#include <hector_uav_msgs/ThrustCommand.h>
namespace hector_quadrotor_controller {
class QuadrotorInterface;
using geometry_msgs::Pose;
using geometry_msgs::Point;
using geometry_msgs::Quaternion;
using geometry_msgs::Twist;
using geometry_msgs::Vector3;
using geometry_msgs::Wrench;
using sensor_msgs::Imu;
using hector_uav_msgs::MotorStatus;
using hector_uav_msgs::MotorCommand;
using hector_uav_msgs::AttitudeCommand;
using hector_uav_msgs::YawrateCommand;
using hector_uav_msgs::ThrustCommand;
template <class Derived, typename T>
class Handle_
{
public:
typedef T ValueType;
typedef Handle_<Derived, T> Base;
Handle_(const std::string& name, const std::string& field = std::string()) : interface_(0), name_(name), field_(field), value_(0) {}
Handle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field), value_(0) {}
Handle_(QuadrotorInterface *interface, const ValueType *source, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field) { *this = source; }
virtual ~Handle_() {}
virtual const std::string& getName() const { return name_; }
virtual const std::string& getField() const { return field_; }
virtual bool connected() const { return get(); }
virtual void reset() { value_ = 0; }
Derived& operator=(const ValueType *source) { value_ = source; return static_cast<Derived &>(*this); }
const ValueType *get() const { return value_; }
const ValueType &operator*() const { return *value_; }
protected:
QuadrotorInterface *interface_;
const std::string name_;
const std::string field_;
const ValueType *value_;
};
class PoseHandle : public Handle_<PoseHandle, Pose>
{
public:
using Base::operator=;
PoseHandle() : Base("pose") {}
PoseHandle(QuadrotorInterface *interface) : Base(interface, "pose") {}
PoseHandle(QuadrotorInterface *interface, const Pose *pose) : Base(interface, pose, "pose") {}
virtual ~PoseHandle() {}
const ValueType& pose() const { return *get(); }
void getEulerRPY(double &roll, double &pitch, double &yaw) const;
double getYaw() const;
Vector3 toBody(const Vector3& nav) const;
Vector3 fromBody(const Vector3& nav) const;
};
typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
class TwistHandle : public Handle_<TwistHandle, Twist>
{
public:
using Base::operator=;
TwistHandle() : Base("twist") {}
TwistHandle(QuadrotorInterface *interface) : Base(interface, "twist") {}
TwistHandle(QuadrotorInterface *interface, const Twist *twist) : Base(interface, twist, "twist") {}
virtual ~TwistHandle() {}
const ValueType& twist() const { return *get(); }
};
typedef boost::shared_ptr<TwistHandle> TwistHandlePtr;
class AccelerationHandle : public Handle_<AccelerationHandle, Vector3>
{
public:
using Base::operator=;
AccelerationHandle(QuadrotorInterface *interface, const Vector3 *acceleration) : Base(interface, acceleration, "acceleration") {}
virtual ~AccelerationHandle() {}
const ValueType& acceleration() const { return *get(); }
};
typedef boost::shared_ptr<AccelerationHandle> AccelerationHandlePtr;
class StateHandle : public PoseHandle, public TwistHandle
{
public:
StateHandle() {}
StateHandle(QuadrotorInterface *interface, const Pose *pose, const Twist *twist) : PoseHandle(interface, pose), TwistHandle(interface, twist) {}
virtual ~StateHandle() {}
virtual bool connected() const { return PoseHandle::connected() && TwistHandle::connected(); }
};
typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
class ImuHandle : public Handle_<ImuHandle, Imu>
{
public:
using Base::operator=;
ImuHandle() : Base("imu") {}
ImuHandle(QuadrotorInterface *interface, const Imu *imu) : Base(interface, imu, "imu") {}
virtual ~ImuHandle() {}
const ValueType& imu() const { return *get(); }
};
typedef boost::shared_ptr<ImuHandle> ImuHandlePtr;
class MotorStatusHandle : public Handle_<MotorStatusHandle, MotorStatus>
{
public:
using Base::operator=;
MotorStatusHandle() : Base("motor_status") {}
MotorStatusHandle(QuadrotorInterface *interface, const MotorStatus *motor_status) : Base(interface, motor_status, "motor_status") {}
virtual ~MotorStatusHandle() {}
const ValueType& motorStatus() const { return *get(); }
};
typedef boost::shared_ptr<MotorStatusHandle> MotorStatusHandlePtr;
class CommandHandle
{
public:
CommandHandle() : interface_(0), new_value_(false) {}
CommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field) : interface_(interface), name_(name), field_(field), new_value_(false) {}
virtual ~CommandHandle() {}
virtual const std::string& getName() const { return name_; }
virtual const std::string& getField() const { return field_; }
virtual bool connected() const = 0;
virtual void reset() {}
void *get() const { return 0; }
bool enabled();
bool start();
void stop();
void disconnect();
template <typename T> T* ownData(T* data) { my_.reset(data); return data; }
template <typename Derived> bool connectFrom(const Derived& output) {
Derived *me = dynamic_cast<Derived *>(this);
if (!me) return false;
ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", output.getName().c_str(), &output, me->getName().c_str(), me);
return (*me = output.get()).connected();
}
template <typename Derived> bool connectTo(Derived& input) const {
const Derived *me = dynamic_cast<const Derived *>(this);
if (!me) return false;
ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", me->getName().c_str(), me, input.getName().c_str(), &input);
return (input = me->get()).connected();
}
private:
QuadrotorInterface *interface_;
const std::string name_;
const std::string field_;
boost::shared_ptr<void> my_;
protected:
mutable bool new_value_;
bool wasNew() const { bool old = new_value_; new_value_ = false; return old; }
};
typedef boost::shared_ptr<CommandHandle> CommandHandlePtr;
namespace internal {
template <class Derived> struct FieldAccessor {
static typename Derived::ValueType *get(void *) { return 0; }
};
}
template <class Derived, typename T, class Parent = CommandHandle>
class CommandHandle_ : public Parent
{
public:
typedef T ValueType;
typedef CommandHandle_<Derived, T, Parent> Base;
CommandHandle_() : command_(0) {}
CommandHandle_(const Parent &other) : Parent(other), command_(0) {}
CommandHandle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Parent(interface, name, field), command_(0) {}
virtual ~CommandHandle_() {}
virtual bool connected() const { return get(); }
virtual void reset() { command_ = 0; Parent::reset(); }
using Parent::operator=;
Derived& operator=(ValueType *source) { command_ = source; return static_cast<Derived &>(*this); }
ValueType *get() const { return command_ ? command_ : internal::FieldAccessor<Derived>::get(Parent::get()); }
ValueType &operator*() const { return *command_; }
ValueType& command() { return *get(); }
const ValueType& getCommand() const { this->new_value_ = false; return *get(); }
void setCommand(const ValueType& command) { this->new_value_ = true; *get() = command; }
bool getCommand(ValueType& command) const { command = *get(); return this->wasNew(); }
bool update(ValueType& command) const {
if (!connected()) return false;
command = getCommand();
return true;
}
protected:
ValueType *command_;
};
class PoseCommandHandle : public CommandHandle_<PoseCommandHandle, Pose>
{
public:
using Base::operator=;
PoseCommandHandle() {}
PoseCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
PoseCommandHandle(Pose* command) { *this = command; }
virtual ~PoseCommandHandle() {}
};
typedef boost::shared_ptr<PoseCommandHandle> PoseCommandHandlePtr;
class HorizontalPositionCommandHandle : public CommandHandle_<HorizontalPositionCommandHandle, Point, PoseCommandHandle>
{
public:
using Base::operator=;
HorizontalPositionCommandHandle() {}
HorizontalPositionCommandHandle(const PoseCommandHandle& other) : Base(other) {}
HorizontalPositionCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.xy") {}
HorizontalPositionCommandHandle(Point* command) { *this = command; }
virtual ~HorizontalPositionCommandHandle() {}
using Base::getCommand;
virtual bool getCommand(double &x, double &y) const {
x = get()->x;
y = get()->y;
return wasNew();
}
using Base::setCommand;
virtual void setCommand(double x, double y)
{
this->new_value_ = true;
get()->x = x;
get()->y = y;
}
using Base::update;
bool update(Pose& command) const {
if (!connected()) return false;
getCommand(command.position.x, command.position.y);
return true;
}
void getError(const PoseHandle &pose, double &x, double &y) const;
};
typedef boost::shared_ptr<HorizontalPositionCommandHandle> HorizontalPositionCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HorizontalPositionCommandHandle> {
static Point *get(Pose *pose) { return &(pose->position); }
};
}
class HeightCommandHandle : public CommandHandle_<HeightCommandHandle, double, PoseCommandHandle>
{
public:
using Base::operator=;
HeightCommandHandle() {}
HeightCommandHandle(const PoseCommandHandle& other) : Base(other) {}
HeightCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.z") {}
HeightCommandHandle(double *command) { *this = command; }
virtual ~HeightCommandHandle() {}
using Base::update;
bool update(Pose& command) const {
if (!connected()) return false;
command.position.z = getCommand();
return true;
}
double getError(const PoseHandle &pose) const;
};
typedef boost::shared_ptr<HeightCommandHandle> HeightCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HeightCommandHandle> {
static double *get(Pose *pose) { return &(pose->position.z); }
};
}
class HeadingCommandHandle : public CommandHandle_<HeadingCommandHandle, Quaternion, PoseCommandHandle>
{
public:
using Base::operator=;
HeadingCommandHandle() {}
HeadingCommandHandle(const PoseCommandHandle& other) : Base(other), scalar_(0) {}
HeadingCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "orientation.yaw"), scalar_(0) {}
HeadingCommandHandle(Quaternion *command) { *this = command; }
virtual ~HeadingCommandHandle() {}
using Base::getCommand;
double getCommand() const;
using Base::setCommand;
void setCommand(double command);
using Base::update;
bool update(Pose& command) const;
double getError(const PoseHandle &pose) const;
protected:
double *scalar_;
};
typedef boost::shared_ptr<HeadingCommandHandle> HeadingCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HeadingCommandHandle> {
static Quaternion *get(Pose *pose) { return &(pose->orientation); }
};
}
class TwistCommandHandle : public CommandHandle_<TwistCommandHandle, Twist>
{
public:
using Base::operator=;
TwistCommandHandle() {}
TwistCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
TwistCommandHandle(Twist* command) { *this = command; }
virtual ~TwistCommandHandle() {}
};
typedef boost::shared_ptr<TwistCommandHandle> TwistCommandHandlePtr;
class HorizontalVelocityCommandHandle : public CommandHandle_<HorizontalVelocityCommandHandle, Vector3, TwistCommandHandle>
{
public:
using Base::operator=;
HorizontalVelocityCommandHandle() {}
HorizontalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
HorizontalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.xy") {}
HorizontalVelocityCommandHandle(Vector3* command) { *this = command; }
virtual ~HorizontalVelocityCommandHandle() {}
using Base::getCommand;
bool getCommand(double &x, double &y) const {
x = get()->x;
y = get()->y;
return true;
}
using Base::setCommand;
void setCommand(double x, double y)
{
get()->x = x;
get()->y = y;
}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
getCommand(command.linear.x, command.linear.y);
return true;
}
};
typedef boost::shared_ptr<HorizontalVelocityCommandHandle> HorizontalVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HorizontalVelocityCommandHandle> {
static Vector3 *get(Twist *twist) { return &(twist->linear); }
};
}
class VerticalVelocityCommandHandle : public CommandHandle_<VerticalVelocityCommandHandle, double, TwistCommandHandle>
{
public:
using Base::operator=;
VerticalVelocityCommandHandle() {}
VerticalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
VerticalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.z") {}
VerticalVelocityCommandHandle(double* command) { *this = command; }
virtual ~VerticalVelocityCommandHandle() {}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
command.linear.z = *get();
return true;
}
};
typedef boost::shared_ptr<VerticalVelocityCommandHandle> VerticalVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<VerticalVelocityCommandHandle> {
static double *get(Twist *twist) { return &(twist->linear.z); }
};
}
class AngularVelocityCommandHandle : public CommandHandle_<AngularVelocityCommandHandle, double, TwistCommandHandle>
{
public:
using Base::operator=;
AngularVelocityCommandHandle() {}
AngularVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
AngularVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "angular.z") {}
AngularVelocityCommandHandle(double* command) { *this = command; }
virtual ~AngularVelocityCommandHandle() {}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
command.linear.z = *get();
return true;
}
};
typedef boost::shared_ptr<AngularVelocityCommandHandle> AngularVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<AngularVelocityCommandHandle> {
static double *get(Twist *twist) { return &(twist->angular.z); }
};
}
class WrenchCommandHandle : public CommandHandle_<WrenchCommandHandle, Wrench>
{
public:
using Base::operator=;
WrenchCommandHandle() {}
WrenchCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~WrenchCommandHandle() {}
};
typedef boost::shared_ptr<WrenchCommandHandle> WrenchCommandHandlePtr;
class MotorCommandHandle : public CommandHandle_<MotorCommandHandle, MotorCommand>
{
public:
using Base::operator=;
MotorCommandHandle() {}
MotorCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~MotorCommandHandle() {}
};
typedef boost::shared_ptr<MotorCommandHandle> MotorCommandHandlePtr;
class AttitudeCommandHandle : public CommandHandle_<AttitudeCommandHandle, AttitudeCommand>
{
public:
using Base::operator=;
AttitudeCommandHandle() {}
AttitudeCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~AttitudeCommandHandle() {}
};
typedef boost::shared_ptr<AttitudeCommandHandle> AttitudeCommandHandlePtr;
class YawrateCommandHandle : public CommandHandle_<YawrateCommandHandle, YawrateCommand>
{
public:
using Base::operator=;
YawrateCommandHandle() {}
YawrateCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~YawrateCommandHandle() {}
};
typedef boost::shared_ptr<YawrateCommandHandle> YawrateCommandHandlePtr;
class ThrustCommandHandle : public CommandHandle_<ThrustCommandHandle, ThrustCommand>
{
public:
using Base::operator=;
ThrustCommandHandle() {}
ThrustCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~ThrustCommandHandle() {}
};
typedef boost::shared_ptr<ThrustCommandHandle> ThrustCommandHandlePtr;
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H

45
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h
View File

@ -1,45 +0,0 @@
#ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H
#define HECTOR_QUADROTOR_CONTROLLER_PID_H
#include <ros/node_handle.h>
namespace hector_quadrotor_controller {
class PID
{
public:
struct parameters {
parameters();
bool enabled;
double time_constant;
double k_p;
double k_i;
double k_d;
double limit_i;
double limit_output;
} parameters_;
struct state {
state();
double p, i, d;
double input, dinput;
double dx;
} state_;
public:
PID();
PID(const parameters& parameters);
~PID();
void init(const ros::NodeHandle &param_nh);
void reset();
double update(double input, double x, double dx, const ros::Duration& dt);
double update(double error, double dx, const ros::Duration& dt);
double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt);
};
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_PID_H

143
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h
View File

@ -1,143 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <hector_quadrotor_controller/handles.h>
#include <map>
#include <list>
namespace hector_quadrotor_controller {
using namespace hardware_interface;
class QuadrotorInterface : public HardwareInterface
{
public:
QuadrotorInterface();
virtual ~QuadrotorInterface();
virtual PoseHandlePtr getPose() { return PoseHandlePtr(); }
virtual TwistHandlePtr getTwist() { return TwistHandlePtr(); }
virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(); }
virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(); }
virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(); }
virtual bool getMassAndInertia(double &mass, double inertia[3]) { return false; }
template <typename HandleType> boost::shared_ptr<HandleType> getHandle()
{
return boost::shared_ptr<HandleType>(new HandleType(this));
}
template <typename HandleType> boost::shared_ptr<HandleType> addInput(const std::string& name)
{
boost::shared_ptr<HandleType> input = getInput<HandleType>(name);
if (input) return input;
// create new input handle
input.reset(new HandleType(this, name));
inputs_[name] = input;
// connect to output of same name
if (outputs_.count(name)) {
boost::shared_ptr<HandleType> output = boost::dynamic_pointer_cast<HandleType>(outputs_.at(name));
output->connectTo(*input);
}
return input;
}
template <typename HandleType> boost::shared_ptr<HandleType> addOutput(const std::string& name)
{
boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
if (output) return output;
// create new output handle
output.reset(new HandleType(this, name));
outputs_[name] = output;
*output = output->ownData(new typename HandleType::ValueType());
//claim resource
claim(name);
// connect to output of same name
if (inputs_.count(name)) {
boost::shared_ptr<HandleType> input = boost::dynamic_pointer_cast<HandleType>(inputs_.at(name));
output->connectTo(*input);
}
return output;
}
template <typename HandleType> boost::shared_ptr<HandleType> getOutput(const std::string& name) const
{
if (!outputs_.count(name)) return boost::shared_ptr<HandleType>();
return boost::static_pointer_cast<HandleType>(outputs_.at(name));
}
template <typename HandleType> boost::shared_ptr<HandleType> getInput(const std::string& name) const
{
if (!inputs_.count(name)) return boost::shared_ptr<HandleType>();
return boost::static_pointer_cast<HandleType>(inputs_.at(name));
}
template <typename HandleType> typename HandleType::ValueType const* getCommand(const std::string& name) const
{
boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
if (!output || !output->connected()) return 0;
return &(output->command());
}
virtual const Pose *getPoseCommand() const;
virtual const Twist *getTwistCommand() const;
virtual const Wrench *getWrenchCommand() const;
virtual const MotorCommand *getMotorCommand() const;
bool enabled(const CommandHandle *handle) const;
bool start(const CommandHandle *handle);
void stop(const CommandHandle *handle);
void disconnect(const CommandHandle *handle);
private:
// typedef std::list< CommandHandlePtr > HandleList;
// std::map<const std::type_info *, HandleList> inputs_;
// std::map<const std::type_info *, HandleList> outputs_;
std::map<std::string, CommandHandlePtr> inputs_;
std::map<std::string, CommandHandlePtr> outputs_;
std::map<std::string, const CommandHandle *> enabled_;
};
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H

7
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch
View File

@ -1,7 +0,0 @@
<launch>
<rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
controller/twist
--shutdown-timeout 3"/>
</launch>

42
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/package.xml
View File

@ -1,42 +0,0 @@
<package>
<name>hector_quadrotor_controller</name>
<version>0.3.5</version>
<description>hector_quadrotor_controller provides libraries and a node for quadrotor control using <a href="http://wiki.ros.org/ros_control">ros_control</a>.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_controller</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>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>hector_uav_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>hardware_interface</build_depend>
<build_depend>controller_interface</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>hector_uav_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_interface</run_depend>
<!-- Dependencies needed only for running tests. -->
<export>
<controller_interface plugin="${prefix}/plugin.xml"/>
</export>
</package>

50
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml
View File

@ -1,50 +0,0 @@
controller:
pose:
type: hector_quadrotor_controller/PoseController
xy:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 5.0
z:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 5.0
yaw:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 1.0
twist:
type: hector_quadrotor_controller/TwistController
linear/xy:
k_p: 5.0
k_i: 1.0
k_d: 0.0
limit_output: 10.0
time_constant: 0.05
linear/z:
k_p: 5.0
k_i: 1.0
k_d: 0.0
limit_output: 10.0
time_constant: 0.05
angular/xy:
k_p: 10.0
k_i: 5.0
k_d: 5.0
time_constant: 0.01
angular/z:
k_p: 5.0
k_i: 2.5
k_d: 0.0
limit_output: 3.0
time_constant: 0.1
limits:
load_factor: 1.5
force/z: 30.0
torque/xy: 10.0
torque/z: 1.0
motor:
type: hector_quadrotor_controller/MotorController

23
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml
View File

@ -1,23 +0,0 @@
<class_libraries>
<library path="lib/libhector_quadrotor_pose_controller">
<class name="hector_quadrotor_controller/PoseController" type="hector_quadrotor_controller::PoseController" base_class_type="controller_interface::ControllerBase">
<description>
The PoseController controls the quadrotor's pose and outputs velocity commands.
</description>
</class>
</library>
<library path="lib/libhector_quadrotor_twist_controller">
<class name="hector_quadrotor_controller/TwistController" type="hector_quadrotor_controller::TwistController" base_class_type="controller_interface::ControllerBase">
<description>
The TwistController controls the quadrotor's twist (linear and angular velocity) by applying torque and thrust similar to a real quadrotor.
</description>
</class>
</library>
<library path="lib/libhector_quadrotor_motor_controller">
<class name="hector_quadrotor_controller/MotorController" type="hector_quadrotor_controller::MotorController" base_class_type="controller_interface::ControllerBase">
<description>
The MotorController calculates the output voltages of the four motors from the commanded thrust and torque using a simple linearized model.
</description>
</class>
</library>
</class_libraries>

194
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp
View File

@ -1,194 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_interface.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class MotorController : public controller_interface::Controller<QuadrotorInterface>
{
public:
MotorController()
: node_handle_(0)
{}
~MotorController()
{
if (node_handle_) {
node_handle_->shutdown();
delete node_handle_;
node_handle_ = 0;
}
}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
wrench_input_ = interface->addInput<WrenchCommandHandle>("wrench");
motor_output_ = interface->addOutput<MotorCommandHandle>("motor");
// initialize NodeHandle
delete node_handle_;
node_handle_ = new ros::NodeHandle(root_nh);
// load parameters
controller_nh.getParam("force_per_voltage", parameters_.force_per_voltage = 0.559966216);
controller_nh.getParam("torque_per_voltage", parameters_.torque_per_voltage = 7.98598e-3);
controller_nh.getParam("lever", parameters_.lever = 0.275);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// TODO: calculate these parameters from the quadrotor_propulsion parameters
// quadrotor_propulsion:
// k_t: 0.015336864714397
// k_m: -7.011631909766668e-5
// CT2s: -1.3077e-2
// CT1s: -2.5224e-4
// CT0s: 1.538190483976698e-5
return true;
}
void reset()
{
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
motor_.force.assign(4, 0.0);
motor_.torque.assign(4, 0.0);
motor_.frequency.clear();
motor_.voltage.assign(4, 0.0);
}
void wrenchCommandCallback(const geometry_msgs::WrenchStampedConstPtr& command)
{
wrench_ = *command;
if (wrench_.header.stamp.isZero()) wrench_.header.stamp = ros::Time::now();
// start controller if it not running
if (!isRunning()) this->startRequest(wrench_.header.stamp);
}
void starting(const ros::Time &time)
{
reset();
motor_output_->start();
}
void stopping(const ros::Time &time)
{
motor_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
// Get wrench command input
if (wrench_input_->connected() && wrench_input_->enabled()) {
wrench_.wrench = wrench_input_->getCommand();
}
// std::cout << "motor_controller:update" << std::endl; //added by zhangshuai@2018.12.18
// std::cout<<"wrench_.wrench.force.x:"<<wrench_.wrench.force.x<<std::endl;
// std::cout<<"wrench_.wrench.force.y:"<<wrench_.wrench.force.y<<std::endl;
// std::cout<<"wrench_.wrench.force.z:"<<wrench_.wrench.force.z<<std::endl;
// std::cout<<"wrench_.wrench.torque.x:"<<wrench_.wrench.torque.x<<std::endl;
// std::cout<<"wrench_.wrench.torque.y:"<<wrench_.wrench.torque.y<<std::endl;
// std::cout<<"wrench_.wrench.torque.z:"<<wrench_.wrench.torque.z<<std::endl;
// Update output
if (wrench_.wrench.force.z > 0.0) {
double nominal_thrust_per_motor = wrench_.wrench.force.z / 4.0;
motor_.force[0] = nominal_thrust_per_motor - wrench_.wrench.torque.y / 2.0 / parameters_.lever;
motor_.force[1] = nominal_thrust_per_motor - wrench_.wrench.torque.x / 2.0 / parameters_.lever;
motor_.force[2] = nominal_thrust_per_motor + wrench_.wrench.torque.y / 2.0 / parameters_.lever;
motor_.force[3] = nominal_thrust_per_motor + wrench_.wrench.torque.x / 2.0 / parameters_.lever;
double nominal_torque_per_motor = wrench_.wrench.torque.z / 4.0;
motor_.voltage[0] = motor_.force[0] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[1] = motor_.force[1] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[2] = motor_.force[2] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[3] = motor_.force[3] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.torque[0] = motor_.voltage[0] * parameters_.torque_per_voltage;
motor_.torque[1] = motor_.voltage[1] * parameters_.torque_per_voltage;
motor_.torque[2] = motor_.voltage[2] * parameters_.torque_per_voltage;
motor_.torque[3] = motor_.voltage[3] * parameters_.torque_per_voltage;
if (motor_.voltage[0] < 0.0) motor_.voltage[0] = 0.0;
if (motor_.voltage[1] < 0.0) motor_.voltage[1] = 0.0;
if (motor_.voltage[2] < 0.0) motor_.voltage[2] = 0.0;
if (motor_.voltage[3] < 0.0) motor_.voltage[3] = 0.0;
} else {
reset();
}
// set wrench output
motor_.header.stamp = time;
motor_.header.frame_id = "base_link";
motor_output_->setCommand(motor_);
}
private:
WrenchCommandHandlePtr wrench_input_;
MotorCommandHandlePtr motor_output_;
ros::NodeHandle *node_handle_;
ros::Subscriber wrench_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::WrenchStamped wrench_;
hector_uav_msgs::MotorCommand motor_;
std::string base_link_frame_;
struct {
double force_per_voltage; // coefficient for linearized volts to force conversion for a single motor [N / V]
double torque_per_voltage; // coefficient for linearized volts to force conversion for a single motor [Nm / V]
double lever; // the lever arm from origin to the motor axes (symmetry assumption) [m]
} parameters_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::MotorController, controller_interface::ControllerBase)

152
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp
View File

@ -1,152 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/pid.h>
#include <limits>
namespace hector_quadrotor_controller {
PID::state::state()
: p(std::numeric_limits<double>::quiet_NaN())
, i(0.0)
, d(std::numeric_limits<double>::quiet_NaN())
, input(std::numeric_limits<double>::quiet_NaN())
, dinput(0.0)
, dx(std::numeric_limits<double>::quiet_NaN())
{
}
PID::parameters::parameters()
: enabled(true)
, time_constant(0.0)
, k_p(0.0)
, k_i(0.0)
, k_d(0.0)
, limit_i(std::numeric_limits<double>::quiet_NaN())
, limit_output(std::numeric_limits<double>::quiet_NaN())
{
}
PID::PID()
{}
PID::PID(const parameters& params)
: parameters_(params)
{}
PID::~PID()
{}
void PID::init(const ros::NodeHandle &param_nh)
{
param_nh.getParam("enabled", parameters_.enabled);
param_nh.getParam("k_p", parameters_.k_p);
param_nh.getParam("k_i", parameters_.k_i);
param_nh.getParam("k_d", parameters_.k_d);
param_nh.getParam("limit_i", parameters_.limit_i);
param_nh.getParam("limit_output", parameters_.limit_output);
param_nh.getParam("time_constant", parameters_.time_constant);
}
void PID::reset()
{
state_ = state();
}
template <typename T> static inline T& checknan(T& value)
{
if (std::isnan(value)) value = T();
return value;
}
double PID::update(double input, double x, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
double dt_sec = dt.toSec();
// low-pass filter input
if (std::isnan(state_.input)) state_.input = input;
if (dt_sec + parameters_.time_constant > 0.0) {
state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
state_.input = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
}
return update(state_.input - x, dx, dt);
}
double PID::update(double error, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
if (std::isnan(error)) return 0.0;
double dt_sec = dt.toSec();
// integral error
state_.i += error * dt_sec;
if (parameters_.limit_i > 0.0)
{
if (state_.i > parameters_.limit_i) state_.i = parameters_.limit_i;
if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
}
// differential error
if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
} else {
state_.d = -dx;
}
state_.dx = dx;
// proportional error
state_.p = error;
// calculate output...
double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
int antiwindup = 0;
if (parameters_.limit_output > 0.0)
{
if (output > parameters_.limit_output) { output = parameters_.limit_output; antiwindup = 1; }
if (output < -parameters_.limit_output) { output = -parameters_.limit_output; antiwindup = -1; }
}
if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
checknan(output);
return output;
}
double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt)
{
double dt_sec = dt.toSec();
filtered_error = checknan(filtered_error);
if (dt_sec + time_constant > 0.0) {
filtered_error = (time_constant * filtered_error + dt_sec * state_.p) / (dt_sec + time_constant);
}
return filtered_error;
}
} // namespace hector_quadrotor_controller

205
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp
View File

@ -1,205 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class PoseController : public controller_interface::Controller<QuadrotorInterface>
{
public:
PoseController() {}
~PoseController() {}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
pose_input_ = interface->addInput<PoseCommandHandle>("pose");
twist_input_ = interface->addInput<TwistCommandHandle>("pose/twist");
twist_limit_ = interface->addInput<TwistCommandHandle>("pose/twist_limit");
twist_output_ = interface->addOutput<TwistCommandHandle>("twist");
node_handle_ = root_nh;
// subscribe to commanded pose and velocity
pose_subscriber_ = node_handle_.subscribe<geometry_msgs::PoseStamped>("command/pose", 1, boost::bind(&PoseController::poseCommandCallback, this, _1));
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&PoseController::twistCommandCallback, this, _1));
// initialize PID controllers
pid_.x.init(ros::NodeHandle(controller_nh, "xy"));
pid_.y.init(ros::NodeHandle(controller_nh, "xy"));
pid_.z.init(ros::NodeHandle(controller_nh, "z"));
pid_.yaw.init(ros::NodeHandle(controller_nh, "yaw"));
return true;
}
void reset()
{
pid_.x.reset();
pid_.y.reset();
pid_.z.reset();
pid_.yaw.reset();
}
void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr& command)
{
pose_command_ = *command;
if (!(pose_input_->connected())) *pose_input_ = &(pose_command_.pose);
pose_input_->start();
ros::Time start_time = command->header.stamp;
if (start_time.isZero()) start_time = ros::Time::now();
if (!isRunning()) this->startRequest(start_time);
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
{
twist_command_ = *command;
if (!(twist_input_->connected())) *twist_input_ = &(twist_command_.twist);
twist_input_->start();
ros::Time start_time = command->header.stamp;
if (start_time.isZero()) start_time = ros::Time::now();
if (!isRunning()) this->startRequest(start_time);
}
void starting(const ros::Time &time)
{
reset();
twist_output_->start();
}
void stopping(const ros::Time &time)
{
twist_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
// std::cout << "pose_controller:update" << std::endl; //added by zhangshuai@2018.12.18
Twist output;
// check command timeout
// TODO
// return if no pose command is available
if (pose_input_->enabled()) {
// control horizontal position
double error_n, error_w;
HorizontalPositionCommandHandle(*pose_input_).getError(*pose_, error_n, error_w);
output.linear.x = pid_.x.update(error_n, twist_->twist().linear.x, period);
output.linear.y = pid_.y.update(error_w, twist_->twist().linear.y, period);
// control height
output.linear.z = pid_.z.update(HeightCommandHandle(*pose_input_).getError(*pose_), twist_->twist().linear.z, period);
// control yaw angle
output.angular.z = pid_.yaw.update(HeadingCommandHandle(*pose_input_).getError(*pose_), twist_->twist().angular.z, period);
}
// add twist command if available
if (twist_input_->enabled())
{
output.linear.x += twist_input_->getCommand().linear.x;
output.linear.y += twist_input_->getCommand().linear.y;
output.linear.z += twist_input_->getCommand().linear.z;
output.angular.x += twist_input_->getCommand().angular.x;
output.angular.y += twist_input_->getCommand().angular.y;
output.angular.z += twist_input_->getCommand().angular.z;
}
// limit twist
if (twist_limit_->enabled())
{
double linear_xy = sqrt(output.linear.x*output.linear.x + output.linear.y*output.linear.y);
double limit_linear_xy = std::max(twist_limit_->get()->linear.x, twist_limit_->get()->linear.y);
if (limit_linear_xy > 0.0 && linear_xy > limit_linear_xy) {
output.linear.x *= limit_linear_xy / linear_xy;
output.linear.y *= limit_linear_xy / linear_xy;
}
if (twist_limit_->get()->linear.z > 0.0 && fabs(output.linear.z) > twist_limit_->get()->linear.z) {
output.linear.z *= twist_limit_->get()->linear.z / fabs(output.linear.z);
}
double angular_xy = sqrt(output.angular.x*output.angular.x + output.angular.y*output.angular.y);
double limit_angular_xy = std::max(twist_limit_->get()->angular.x, twist_limit_->get()->angular.y);
if (limit_angular_xy > 0.0 && angular_xy > limit_angular_xy) {
output.angular.x *= limit_angular_xy / angular_xy;
output.angular.y *= limit_angular_xy / angular_xy;
}
if (twist_limit_->get()->angular.z > 0.0 && fabs(output.angular.z) > twist_limit_->get()->angular.z) {
output.angular.z *= twist_limit_->get()->angular.z / fabs(output.angular.z);
}
}
// set twist output
twist_output_->setCommand(output);
}
private:
PoseHandlePtr pose_;
PoseCommandHandlePtr pose_input_;
TwistHandlePtr twist_;
TwistCommandHandlePtr twist_input_;
TwistCommandHandlePtr twist_limit_;
TwistCommandHandlePtr twist_output_;
geometry_msgs::PoseStamped pose_command_;
geometry_msgs::TwistStamped twist_command_;
ros::NodeHandle node_handle_;
ros::Subscriber pose_subscriber_;
ros::Subscriber twist_subscriber_;
struct {
PID x;
PID y;
PID z;
PID yaw;
} pid_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::PoseController, controller_interface::ControllerBase)

195
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp
View File

@ -1,195 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_interface.h>
#include <cmath>
namespace hector_quadrotor_controller {
QuadrotorInterface::QuadrotorInterface()
{}
QuadrotorInterface::~QuadrotorInterface()
{}
bool QuadrotorInterface::enabled(const CommandHandle *handle) const
{
if (!handle || !handle->connected()) return false;
std::string resource = handle->getName();
return enabled_.count(resource) > 0;
}
bool QuadrotorInterface::start(const CommandHandle *handle)
{
if (!handle || !handle->connected()) return false;
if (enabled(handle)) return true;
std::string resource = handle->getName();
enabled_[resource] = handle;
ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str());
return true;
}
void QuadrotorInterface::stop(const CommandHandle *handle)
{
if (!handle) return;
if (!enabled(handle)) return;
std::string resource = handle->getName();
std::map<std::string, const CommandHandle *>::iterator it = enabled_.lower_bound(resource);
if (it != enabled_.end() && it->second == handle) enabled_.erase(it);
ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str());
}
void QuadrotorInterface::disconnect(const CommandHandle *handle)
{
if (!handle) return;
std::string resource = handle->getName();
if (inputs_.count(resource)) {
const CommandHandlePtr& input = inputs_.at(resource);
if (input.get() != handle) input->reset();
}
if (outputs_.count(resource)) {
const CommandHandlePtr& output = outputs_.at(resource);
if (output.get() != handle) output->reset();
}
}
const Pose *QuadrotorInterface::getPoseCommand() const { return getCommand<PoseCommandHandle>("pose"); }
const Twist *QuadrotorInterface::getTwistCommand() const { return getCommand<TwistCommandHandle>("twist"); }
const Wrench *QuadrotorInterface::getWrenchCommand() const { return getCommand<WrenchCommandHandle>("wrench"); }
const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand<MotorCommandHandle>("motor"); }
void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
roll = atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w);
pitch = -asin(2.*x*z - 2.*w*y);
yaw = atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
double PoseHandle::getYaw() const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
Vector3 PoseHandle::toBody(const Vector3& nav) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
Vector3 body;
body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z;
body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z;
body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z;
return body;
}
Vector3 PoseHandle::fromBody(const Vector3& body) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
Vector3 nav;
nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z;
nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z;
nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z;
return nav;
}
void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const
{
getCommand(x, y);
x -= pose.get()->position.x;
y -= pose.get()->position.y;
}
double HeightCommandHandle::getError(const PoseHandle &pose) const
{
return getCommand() - pose.get()->position.z;
}
void HeadingCommandHandle::setCommand(double command)
{
if (get()) {
get()->x = 0.0;
get()->y = 0.0;
get()->z = sin(command / 2.);
get()->w = cos(command / 2.);
}
if (scalar_) {
*scalar_ = command;
}
}
double HeadingCommandHandle::getCommand() const {
if (scalar_) return *scalar_;
const Quaternion::_w_type& w = get()->w;
const Quaternion::_x_type& x = get()->x;
const Quaternion::_y_type& y = get()->y;
const Quaternion::_z_type& z = get()->z;
return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
bool HeadingCommandHandle::update(Pose& command) const {
if (get()) {
command.orientation = *get();
return true;
}
if (scalar_) {
command.orientation.x = 0.0;
command.orientation.y = 0.0;
command.orientation.z = sin(*scalar_ / 2.);
command.orientation.x = cos(*scalar_ / 2.);
return true;
}
return false;
}
double HeadingCommandHandle::getError(const PoseHandle &pose) const {
static const double M_2PI = 2.0 * M_PI;
double error = getCommand() - pose.getYaw() + M_PI;
error -= floor(error / M_2PI) * M_2PI;
return error - M_PI;
}
bool CommandHandle::enabled() { return interface_->enabled(this); }
bool CommandHandle::start() { return interface_->start(this); }
void CommandHandle::stop() { interface_->stop(this); }
void CommandHandle::disconnect() { interface_->disconnect(this); }
} // namespace hector_quadrotor_controller

328
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp
View File

@ -1,328 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
#include <boost/thread.hpp>
#include <limits>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class TwistController : public controller_interface::Controller<QuadrotorInterface>
{
public:
TwistController()
{}
~TwistController()
{}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
acceleration_ = interface->getAcceleration();
twist_input_ = interface->addInput<TwistCommandHandle>("twist");
wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
node_handle_ = root_nh;
// subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("/fixedwing/cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
// engage/shutdown service servers
engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
// initialize PID controllers
pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
// load other parameters
controller_nh.getParam("auto_engage", auto_engage_ = true);
controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
controller_nh.getParam("limits/force/z", limits_.force.z);
controller_nh.getParam("limits/torque/xy", limits_.torque.x);
controller_nh.getParam("limits/torque/xy", limits_.torque.y);
controller_nh.getParam("limits/torque/z", limits_.torque.z);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// get mass and inertia from QuadrotorInterface
interface->getMassAndInertia(mass_, inertia_);
command_given_in_stabilized_frame_ = false;
return true;
}
void reset()
{
pid_.linear.x.reset();
pid_.linear.y.reset();
pid_.linear.z.reset();
pid_.angular.x.reset();
pid_.angular.y.reset();
pid_.angular.z.reset();
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
linear_z_control_error_ = 0.0;
motors_running_ = false;
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_ = *command;
if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = false;
// start controller if it not running
if (!isRunning()) this->startRequest(command_.header.stamp);
}
void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_.twist = *command;
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = true;
// start controller if it not running
if (!isRunning()) this->startRequest(command_.header.stamp);
}
bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
motors_running_ = true;
return true;
}
bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
motors_running_ = false;
return true;
}
void starting(const ros::Time &time)
{
reset();
wrench_output_->start();
}
void stopping(const ros::Time &time)
{
wrench_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
boost::mutex::scoped_lock lock(command_mutex_);
// Get twist command input
if (twist_input_->connected() && twist_input_->enabled()) {
command_.twist = twist_input_->getCommand();
command_given_in_stabilized_frame_ = false;
}
// Get current state and command
Twist command = command_.twist;
Twist twist = twist_->twist();
Twist twist_body;
twist_body.linear = pose_->toBody(twist.linear);
twist_body.angular = pose_->toBody(twist.angular);
// Transform to world coordinates if necessary (yaw only)
if (command_given_in_stabilized_frame_) {
double yaw = pose_->getYaw();
Twist transformed = command;
transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
command = transformed;
}
// Get gravity and load factor
const double gravity = 9.8065;
double load_factor = 1. / ( pose_->pose().orientation.w * pose_->pose().orientation.w
- pose_->pose().orientation.x * pose_->pose().orientation.x
- pose_->pose().orientation.y * pose_->pose().orientation.y
+ pose_->pose().orientation.z * pose_->pose().orientation.z );
// Note: load_factor could be NaN or Inf...?
if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit;
// Auto engage/shutdown
if (auto_engage_) {
if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) {
motors_running_ = true;
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
} else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) {
double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) {
motors_running_ = false;
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
} else {
ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
}
} else {
linear_z_control_error_ = 0.0;
}
// flip over?
if (motors_running_ && load_factor < 0.0) {
motors_running_ = false;
ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
}
}
// Update output
if (motors_running_) {
Vector3 acceleration_command;
acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period);
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z;
if (wrench_.wrench.force.z <= std::numeric_limits<double>::min()) wrench_.wrench.force.z = std::numeric_limits<double>::min();
if (limits_.torque.x > 0.0) {
if (wrench_.wrench.torque.x > limits_.torque.x) wrench_.wrench.torque.x = limits_.torque.x;
if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x;
}
if (limits_.torque.y > 0.0) {
if (wrench_.wrench.torque.y > limits_.torque.y) wrench_.wrench.torque.y = limits_.torque.y;
if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y;
}
if (limits_.torque.z > 0.0) {
if (wrench_.wrench.torque.z > limits_.torque.z) wrench_.wrench.torque.z = limits_.torque.z;
if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z;
}
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
} else {
reset();
}
// set wrench output
wrench_.header.stamp = time;
wrench_.header.frame_id = base_link_frame_;
wrench_output_->setCommand(wrench_.wrench);
}
private:
PoseHandlePtr pose_;
TwistHandlePtr twist_;
AccelerationHandlePtr acceleration_;
TwistCommandHandlePtr twist_input_;
WrenchCommandHandlePtr wrench_output_;
ros::NodeHandle node_handle_;
ros::Subscriber twist_subscriber_;
ros::Subscriber cmd_vel_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::TwistStamped command_;
geometry_msgs::WrenchStamped wrench_;
bool command_given_in_stabilized_frame_;
std::string base_link_frame_;
struct {
struct {
PID x;
PID y;
PID z;
} linear, angular;
} pid_;
geometry_msgs::Wrench limits_;
bool auto_engage_;
double load_factor_limit;
double mass_;
double inertia_[3];
bool motors_running_;
double linear_z_control_error_;
boost::mutex command_mutex_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

408
Flight_control/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp
View File

@ -1,408 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
#include <boost/thread.hpp>
#include <limits>
// #ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19
// #define _COUNT_TIME_HH_
// #ifndef _WIN32
// #include <sys/time.h>
// #endif
// #ifndef USE_COUNT_TIME
// #define USE_COUNT_TIME
// #endif
// #endif
namespace hector_quadrotor_controller
{
using namespace controller_interface;
class TwistController : public controller_interface::Controller<QuadrotorInterface>
{
public:
TwistController()
{
}
~TwistController()
{
}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
acceleration_ = interface->getAcceleration();
twist_input_ = interface->addInput<TwistCommandHandle>("twist");
wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
node_handle_ = root_nh;
// subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
// engage/shutdown service servers
engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
// initialize PID controllers
pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
// load other parameters
controller_nh.getParam("auto_engage", auto_engage_ = true);
controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
controller_nh.getParam("limits/force/z", limits_.force.z);
controller_nh.getParam("limits/torque/xy", limits_.torque.x);
controller_nh.getParam("limits/torque/xy", limits_.torque.y);
controller_nh.getParam("limits/torque/z", limits_.torque.z);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// get mass and inertia from QuadrotorInterface
interface->getMassAndInertia(mass_, inertia_);
command_given_in_stabilized_frame_ = false;
return true;
}
void reset()
{
pid_.linear.x.reset();
pid_.linear.y.reset();
pid_.linear.z.reset();
pid_.angular.x.reset();
pid_.angular.y.reset();
pid_.angular.z.reset();
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
linear_z_control_error_ = 0.0;
motors_running_ = false;
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr &command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_ = *command;
if (command_.header.stamp.isZero())
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = false;
// start controller if it not running
if (!isRunning())
this->startRequest(command_.header.stamp);
}
void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr &command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_.twist = *command;
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = true;
// start controller if it not running
if (!isRunning())
this->startRequest(command_.header.stamp);
}
bool engageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
motors_running_ = true;
return true;
}
bool shutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
motors_running_ = false;
return true;
}
void starting(const ros::Time &time)
{
reset();
wrench_output_->start();
}
void stopping(const ros::Time &time)
{
wrench_output_->stop();
}
// int i=0; //added by zhangshuai@2018.12.18
#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
double twist_controller_update_time = 0.0;
int num = 0;
// double UpdateTime = 0;
// double ProcessTime = 0;
// double publishModelPoses_time = 0;
#endif
void update(const ros::Time &time, const ros::Duration &period)
{
#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19
struct timeval tv;
double start_time, cur_time, tmp_time;
gettimeofday(&tv, NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// std::cout<<"update:"<<i<<std::endl; //added by zhangshuai@2018.12.18
// i++; //added by zhangshuai@2018.12.18
boost::mutex::scoped_lock lock(command_mutex_);
// Get twist command input
if (twist_input_->connected() && twist_input_->enabled())
{
command_.twist = twist_input_->getCommand();
command_given_in_stabilized_frame_ = false;
}
// Get current state and command
Twist command = command_.twist;
Twist twist = twist_->twist();
Twist twist_body;
twist_body.linear = pose_->toBody(twist.linear);
twist_body.angular = pose_->toBody(twist.angular);
// Transform to world coordinates if necessary (yaw only)
if (command_given_in_stabilized_frame_)
{
double yaw = pose_->getYaw();
Twist transformed = command;
transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
command = transformed;
}
// Get gravity and load factor
const double gravity = 9.8065;
double load_factor = 1. / (pose_->pose().orientation.w * pose_->pose().orientation.w - pose_->pose().orientation.x * pose_->pose().orientation.x - pose_->pose().orientation.y * pose_->pose().orientation.y + pose_->pose().orientation.z * pose_->pose().orientation.z);
// Note: load_factor could be NaN or Inf...?
if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit))
load_factor = load_factor_limit;
// Auto engage/shutdown
if (auto_engage_)
{
if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0)
{
motors_running_ = true;
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
}
else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */)
{
double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
if (linear_z_control_error_ > 0.0)
linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit)
{
motors_running_ = false;
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
}
else
{
ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
}
}
else
{
linear_z_control_error_ = 0.0;
}
// flip over?
if (motors_running_ && load_factor < 0.0)
{
motors_running_ = false;
ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
}
}
// Update output
if (motors_running_)
{
Vector3 acceleration_command;
acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update(acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update(command.angular.z, twist.angular.z, 0.0, period);
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z)
wrench_.wrench.force.z = limits_.force.z;
if (wrench_.wrench.force.z <= std::numeric_limits<double>::min())
wrench_.wrench.force.z = std::numeric_limits<double>::min();
if (limits_.torque.x > 0.0)
{
if (wrench_.wrench.torque.x > limits_.torque.x)
wrench_.wrench.torque.x = limits_.torque.x;
if (wrench_.wrench.torque.x < -limits_.torque.x)
wrench_.wrench.torque.x = -limits_.torque.x;
}
if (limits_.torque.y > 0.0)
{
if (wrench_.wrench.torque.y > limits_.torque.y)
wrench_.wrench.torque.y = limits_.torque.y;
if (wrench_.wrench.torque.y < -limits_.torque.y)
wrench_.wrench.torque.y = -limits_.torque.y;
}
if (limits_.torque.z > 0.0)
{
if (wrench_.wrench.torque.z > limits_.torque.z)
wrench_.wrench.torque.z = limits_.torque.z;
if (wrench_.wrench.torque.z < -limits_.torque.z)
wrench_.wrench.torque.z = -limits_.torque.z;
}
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
}
else
{
reset();
}
// set wrench output
wrench_.header.stamp = time;
wrench_.header.frame_id = base_link_frame_;
// std::cout << "twist_controller:update" << std::endl; //added by zhangshuai@2018.12.18
// std::cout<<"wrench_.wrench.force.x:"<<wrench_.wrench.force.x<<std::endl;
// std::cout<<"wrench_.wrench.force.y:"<<wrench_.wrench.force.y<<std::endl;
// std::cout<<"wrench_.wrench.force.z:"<<wrench_.wrench.force.z<<std::endl;
// std::cout<<"wrench_.wrench.torque.x:"<<wrench_.wrench.torque.x<<std::endl;
// std::cout<<"wrench_.wrench.torque.y:"<<wrench_.wrench.torque.y<<std::endl;
// std::cout<<"wrench_.wrench.torque.z:"<<wrench_.wrench.torque.z<<std::endl;
wrench_output_->setCommand(wrench_.wrench);
#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
gettimeofday(&tv, NULL);
num++;
twist_controller_update_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
if (num == 9999)
{
std::cout << "twist_controller_update_time: " << twist_controller_update_time << "num:" << num << std::endl;
}
#endif
}
private:
PoseHandlePtr pose_;
TwistHandlePtr twist_;
AccelerationHandlePtr acceleration_;
TwistCommandHandlePtr twist_input_;
WrenchCommandHandlePtr wrench_output_;
ros::NodeHandle node_handle_;
ros::Subscriber twist_subscriber_;
ros::Subscriber cmd_vel_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::TwistStamped command_;
geometry_msgs::WrenchStamped wrench_;
bool command_given_in_stabilized_frame_;
std::string base_link_frame_;
struct
{
struct
{
PID x;
PID y;
PID z;
} linear, angular;
} pid_;
geometry_msgs::Wrench limits_;
bool auto_engage_;
double load_factor_limit;
double mass_;
double inertia_[3];
bool motors_running_;
double linear_z_control_error_;
boost::mutex command_mutex_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

20
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst
View File

@ -1,20 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_controller_gazebo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
0.3.3 (2014-09-01)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-12-26)
------------------
* New controller implementation using ros_control
* Contributors: Johannes Meyer

122
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt
View File

@ -1,122 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor_controller_gazebo)
## 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
gazebo_ros_control
hector_quadrotor_controller
)
include_directories(include ${catkin_INCLUDE_DIRS})
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
# Depend on system install of Gazebo
find_package(gazebo REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${GAZEBO_INCLUDE_DIRS})
## 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()
#######################################
## Declare ROS messages and services ##
#######################################
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
###################################
## 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 include
LIBRARIES hector_quadrotor_controller_gazebo
CATKIN_DEPENDS gazebo_ros_control hector_quadrotor_controller
# DEPENDS system_lib
)
###########
## Build ##
###########
add_library(hector_quadrotor_controller_gazebo
src/quadrotor_hardware_gazebo.cpp
)
target_link_libraries(hector_quadrotor_controller_gazebo
${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}
)
#############
## 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 hector_quadrotor_controller_gazebo
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"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
quadrotor_controller_gazebo.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_hector_quadrotor_controller_gazebo.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

109
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h
View File

@ -1,109 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
#include <gazebo_ros_control/robot_hw_sim.h>
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <hector_uav_msgs/MotorStatus.h>
#include <ros/node_handle.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller_gazebo {
using namespace hector_quadrotor_controller;
using namespace hardware_interface;
using namespace gazebo_ros_control;
class QuadrotorHardwareSim : public RobotHWSim, public QuadrotorInterface
{
public:
QuadrotorHardwareSim();
virtual ~QuadrotorHardwareSim();
virtual const ros::Time &getTimestamp() { return header_.stamp; }
virtual PoseHandlePtr getPose() { return PoseHandlePtr(new PoseHandle(this, &pose_)); }
virtual TwistHandlePtr getTwist() { return TwistHandlePtr(new TwistHandle(this, &twist_)); }
virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(new AccelerationHandle(this, &acceleration_)); }
virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(new ImuHandle(this, &imu_)); }
virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(new MotorStatusHandle(this, &motor_status_)); }
virtual bool getMassAndInertia(double &mass, double inertia[3]);
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions);
virtual void readSim(ros::Time time, ros::Duration period);
virtual void writeSim(ros::Time time, ros::Duration period);
private:
void stateCallback(const nav_msgs::OdometryConstPtr &state);
void imuCallback(const sensor_msgs::ImuConstPtr &imu);
void motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status);
protected:
std_msgs::Header header_;
Pose pose_;
Twist twist_;
Vector3 acceleration_;
Imu imu_;
MotorStatus motor_status_;
std::string base_link_frame_, world_frame_;
WrenchCommandHandlePtr wrench_output_;
MotorCommandHandlePtr motor_output_;
gazebo::physics::ModelPtr model_;
gazebo::physics::LinkPtr link_;
gazebo::physics::PhysicsEnginePtr physics_;
gazebo::math::Pose gz_pose_;
gazebo::math::Vector3 gz_velocity_, gz_acceleration_, gz_angular_velocity_;
ros::CallbackQueue callback_queue_;
ros::Subscriber subscriber_state_;
ros::Subscriber subscriber_imu_;
ros::Subscriber subscriber_motor_status_;
ros::Publisher publisher_wrench_command_;
ros::Publisher publisher_motor_command_;
};
} // namespace hector_quadrotor_controller_gazebo
#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H

24
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml
View File

@ -1,24 +0,0 @@
<?xml version="1.0"?>
<package>
<name>hector_quadrotor_controller_gazebo</name>
<version>0.3.5</version>
<description>The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_controller_gazebo</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>
<buildtool_depend>catkin</buildtool_depend>
<build_depend version_gte="2.3.4">gazebo_ros_control</build_depend>
<build_depend>hector_quadrotor_controller</build_depend>
<run_depend version_gte="2.3.4">gazebo_ros_control</run_depend>
<run_depend>hector_quadrotor_controller</run_depend>
<export>
<gazebo_ros_control plugin="${prefix}/quadrotor_controller_gazebo.xml"/>
</export>
</package>

7
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml
View File

@ -1,7 +0,0 @@
<library path="lib/libhector_quadrotor_controller_gazebo">
<class name="hector_quadrotor_controller_gazebo/QuadrotorHardwareSim" type="hector_quadrotor_controller_gazebo::QuadrotorHardwareSim" base_class_type="gazebo_ros_control::RobotHWSim">
<description>
This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin.
</description>
</class>
</library>

275
Flight_control/src/hector_quadrotor/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
View File

@ -1,275 +0,0 @@
//=================================================================================================
// Copyright (c) 2013, 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 <hector_quadrotor_controller/quadrotor_hardware_gazebo.h>
#include <geometry_msgs/WrenchStamped.h>
#ifndef _COUNT_TIME_HH_ //Added by zhangshuai@2018.12.19
#define _COUNT_TIME_HH_
#ifndef _WIN32
#include <sys/time.h>
#endif
#ifndef USE_COUNT_TIME
#define USE_COUNT_TIME
#endif
#endif
namespace hector_quadrotor_controller_gazebo
{
QuadrotorHardwareSim::QuadrotorHardwareSim()
{
this->registerInterface(static_cast<QuadrotorInterface *>(this));
wrench_output_ = addInput<WrenchCommandHandle>("wrench");
motor_output_ = addInput<MotorCommandHandle>("motor");
}
QuadrotorHardwareSim::~QuadrotorHardwareSim()
{
}
bool QuadrotorHardwareSim::initSim(
const std::string &robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
ros::NodeHandle param_nh(model_nh, "controller");
// store parent model pointer
model_ = parent_model;
link_ = model_->GetLink();
physics_ = model_->GetWorld()->GetPhysicsEngine();
model_nh.param<std::string>("world_frame", world_frame_, "world");
model_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// subscribe state
std::string state_topic;
param_nh.getParam("state_topic", state_topic);
if (!state_topic.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(state_topic, 1, boost::bind(&QuadrotorHardwareSim::stateCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_state_ = model_nh.subscribe(ops);
gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_state_.getTopic() << "' as state input for control" << std::endl;
}
else
{
gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as state input for control" << std::endl;
}
// subscribe imu
std::string imu_topic;
param_nh.getParam("imu_topic", imu_topic);
if (!imu_topic.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(imu_topic, 1, boost::bind(&QuadrotorHardwareSim::imuCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_imu_ = model_nh.subscribe(ops);
gzlog << "[hector_quadrotor_controller_gazebo] Using topic '" << subscriber_imu_.getTopic() << "' as imu input for control" << std::endl;
}
else
{
gzlog << "[hector_quadrotor_controller_gazebo] Using ground truth from Gazebo as imu input for control" << std::endl;
}
// subscribe motor_status
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<hector_uav_msgs::MotorStatus>("motor_status", 1, boost::bind(&QuadrotorHardwareSim::motorStatusCallback, this, _1), ros::VoidConstPtr(), &callback_queue_);
subscriber_motor_status_ = model_nh.subscribe(ops);
}
// publish wrench
{
ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::WrenchStamped>("command/wrench", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
publisher_wrench_command_ = model_nh.advertise(ops);
}
// publish motor command
{
ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<hector_uav_msgs::MotorCommand>("command/motor", 1, ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_);
publisher_motor_command_ = model_nh.advertise(ops);
}
return true;
}
bool QuadrotorHardwareSim::getMassAndInertia(double &mass, double inertia[3])
{
if (!link_)
return false;
mass = link_->GetInertial()->GetMass();
gazebo::math::Vector3 Inertia = link_->GetInertial()->GetPrincipalMoments();
inertia[0] = Inertia.x;
inertia[1] = Inertia.y;
inertia[2] = Inertia.z;
return true;
}
void QuadrotorHardwareSim::stateCallback(const nav_msgs::OdometryConstPtr &state)
{
// calculate acceleration
if (!header_.stamp.isZero() && !state->header.stamp.isZero())
{
const double acceleration_time_constant = 0.1;
double dt((state->header.stamp - header_.stamp).toSec());
if (dt > 0.0)
{
acceleration_.x = ((state->twist.twist.linear.x - twist_.linear.x) + acceleration_time_constant * acceleration_.x) / (dt + acceleration_time_constant);
acceleration_.y = ((state->twist.twist.linear.y - twist_.linear.y) + acceleration_time_constant * acceleration_.y) / (dt + acceleration_time_constant);
acceleration_.z = ((state->twist.twist.linear.z - twist_.linear.z) + acceleration_time_constant * acceleration_.z) / (dt + acceleration_time_constant);
}
}
header_ = state->header;
pose_ = state->pose.pose;
twist_ = state->twist.twist;
}
void QuadrotorHardwareSim::imuCallback(const sensor_msgs::ImuConstPtr &imu)
{
imu_ = *imu;
}
void QuadrotorHardwareSim::motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status)
{
motor_status_ = *motor_status;
}
void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
{
// call all available subscriber callbacks now
callback_queue_.callAvailable();
// read state from Gazebo
const double acceleration_time_constant = 0.1;
gz_pose_ = link_->GetWorldPose();
gz_acceleration_ = ((link_->GetWorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) / (period.toSec() + acceleration_time_constant);
gz_velocity_ = link_->GetWorldLinearVel();
gz_angular_velocity_ = link_->GetWorldAngularVel();
if (!subscriber_state_)
{
header_.frame_id = world_frame_;
header_.stamp = time;
pose_.position.x = gz_pose_.pos.x;
pose_.position.y = gz_pose_.pos.y;
pose_.position.z = gz_pose_.pos.z;
pose_.orientation.w = gz_pose_.rot.w;
pose_.orientation.x = gz_pose_.rot.x;
pose_.orientation.y = gz_pose_.rot.y;
pose_.orientation.z = gz_pose_.rot.z;
twist_.linear.x = gz_velocity_.x;
twist_.linear.y = gz_velocity_.y;
twist_.linear.z = gz_velocity_.z;
twist_.angular.x = gz_angular_velocity_.x;
twist_.angular.y = gz_angular_velocity_.y;
twist_.angular.z = gz_angular_velocity_.z;
acceleration_.x = gz_acceleration_.x;
acceleration_.y = gz_acceleration_.y;
acceleration_.z = gz_acceleration_.z;
}
if (!subscriber_imu_)
{
imu_.orientation.w = gz_pose_.rot.w;
imu_.orientation.x = gz_pose_.rot.x;
imu_.orientation.y = gz_pose_.rot.y;
imu_.orientation.z = gz_pose_.rot.z;
gazebo::math::Vector3 gz_angular_velocity_body = gz_pose_.rot.RotateVectorReverse(gz_angular_velocity_);
imu_.angular_velocity.x = gz_angular_velocity_body.x;
imu_.angular_velocity.y = gz_angular_velocity_body.y;
imu_.angular_velocity.z = gz_angular_velocity_body.z;
gazebo::math::Vector3 gz_linear_acceleration_body = gz_pose_.rot.RotateVectorReverse(gz_acceleration_ - physics_->GetGravity());
imu_.linear_acceleration.x = gz_linear_acceleration_body.x;
imu_.linear_acceleration.y = gz_linear_acceleration_body.y;
imu_.linear_acceleration.z = gz_linear_acceleration_body.z;
}
}
#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
double QuadrotorHardwareSim_writeSim_time = 0.0;
int num = 0;
#endif
void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
{
#ifdef USE_COUNT_TIME //Added by zhangshuai@2018.12.19
struct timeval tv;
double start_time, cur_time, tmp_time;
gettimeofday(&tv, NULL);
start_time = cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
bool result_written = false;
if (motor_output_->connected() && motor_output_->enabled())
{
publisher_motor_command_.publish(motor_output_->getCommand());
result_written = true;
}
if (wrench_output_->connected() && wrench_output_->enabled())
{
geometry_msgs::WrenchStamped wrench;
wrench.header.stamp = time;
wrench.header.frame_id = base_link_frame_;
wrench.wrench = wrench_output_->getCommand();
publisher_wrench_command_.publish(wrench);
if (!result_written)
{
// std::cout << "-----------QuadrotorHardwareSim------------" << std::endl; //added by zhangshuai@2018.12.20
gazebo::math::Vector3 force(wrench.wrench.force.x, wrench.wrench.force.y, wrench.wrench.force.z);
gazebo::math::Vector3 torque(wrench.wrench.torque.x, wrench.wrench.torque.y, wrench.wrench.torque.z);
link_->AddRelativeForce(force);
link_->AddRelativeTorque(torque - link_->GetInertial()->GetCoG().Cross(force));
}
}
#ifdef USE_COUNT_TIME //added by zhangshuai@2018.12.19
// gettimeofday(&tv, NULL);
// QuadrotorHardwareSim_writeSim_time += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
// num++;
// if (num == 100000)
// {
// std::cout << "QuadrotorHardwareSim_writeSim_time: " << QuadrotorHardwareSim_writeSim_time << "num:" << num << std::endl;
// }
#endif
}
} // namespace hector_quadrotor_controller_gazebo
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller_gazebo::QuadrotorHardwareSim, gazebo_ros_control::RobotHWSim)

25
Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CHANGELOG.rst
View File

@ -1,25 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_demo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
0.3.3 (2014-09-01)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-12-26)
------------------
* updated demo launch files
* updated rviz configs
* 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

67
Flight_control/src/hector_quadrotor/hector_quadrotor_demo/CMakeLists.txt
View File

@ -1,67 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor_demo)
## 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)
## 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()
#############
## 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 @{name} @{name}_node
# 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})
install(DIRECTORY rviz_cfg DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

26
Flight_control/src/hector_quadrotor/hector_quadrotor_demo/launch/indoor_slam_gazebo.launch
View File

@ -1,26 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Start Gazebo with wg world running in (max) realtime -->
<include file="$(find hector_gazebo_worlds)/launch/willow_garage.launch"/>
<!-- Spawn simulated quadrotor uav -->
<include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch" >
<arg name="model" value="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>
</include>
<!-- Start SLAM system -->
<include file="$(find hector_mapping)/launch/mapping_default.launch">
<arg name="odom_frame" value="world"/>
</include>
<!-- Start GeoTIFF mapper -->
<include file="$(find hector_geotiff)/launch/geotiff_mapper.launch">
<arg name="trajectory_publish_rate" value="4"/>
</include>
<!-- Start rviz visualization with preset config -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find hector_quadrotor_demo)/rviz_cfg/indoor_slam.rviz"/>
</launch>

Some files were not shown because too many files have changed in this diff Show More