dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ros: add offboard attitude sp demo (WIP)

This commit is contained in:
Thomas Gubler 2015-02-17 22:03:20 +01:00
parent 80fbb512c9
commit 27511324ff
4 changed files with 160 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

@ -263,6 +263,15 @@ target_link_libraries(demo_offboard_position_setpoints
px4
)
## Offboard Attitude Setpoint Demo
add_executable(demo_offboard_attitude_setpoints
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp)
add_dependencies(demo_offboard_attitude_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp)
target_link_libraries(demo_offboard_attitude_setpoints
${catkin_LIBRARIES}
px4
)
#############
## Install ##
#############

10
launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch Normal file
View File

@ -0,0 +1,10 @@
<launch>
<include file="$(find px4)/launch/gazebo_ardrone_empty_world.launch" />
<include file="$(find px4)/launch/mavros_sitl.launch" />
<group ns="px4_multicopter">
<node pkg="px4" name="demo_offboard_position_setpoints" type="demo_offboard_attitude_setpoints"/>
</group>
</launch>

83
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp Normal file
View File

@ -0,0 +1,83 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 OWNER OR CONTRIBUTORS 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.
*
****************************************************************************/
/**
* @file demo_offboard_position_Setpoints.cpp
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "demo_offboard_attitude_setpoints.h"
#include <platforms/px4_middleware.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Float64.h>
DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() :
_n(),
_attitude_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/attitude", 1)),
_thrust_sp_pub(_n.advertise<std_msgs::Float64>("mavros/setpoint/att_throttle", 1))
{
}
int DemoOffboardAttitudeSetpoints::main()
{
px4::Rate loop_rate(10);
while (ros::ok()) {
loop_rate.sleep();
ros::spinOnce();
/* Publish example offboard attitude setpoint */
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1;
_attitude_sp_pub.publish(pose);
std_msgs::Float64 thrust;
thrust.data = 0.5;
_thrust_sp_pub.publish(thrust);
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "demo_offboard_position_setpoints");
DemoOffboardAttitudeSetpoints d;
return d.main();
}

58
src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h Normal file
View File

@ -0,0 +1,58 @@
/****************************************************************************
*
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. 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.
* 3. Neither the name PX4 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 OWNER OR CONTRIBUTORS 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.
*
****************************************************************************/
/**
* @file demo_offboard_attitude_Setpoints.h
*
* Demo for sending offboard position setpoints to mavros to show offboard position control in SITL
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include "ros/ros.h"
#include <px4/manual_control_setpoint.h>
class DemoOffboardAttitudeSetpoints
{
public:
DemoOffboardAttitudeSetpoints();
~DemoOffboardAttitudeSetpoints() {}
int main();
protected:
ros::NodeHandle _n;
ros::Publisher _attitude_sp_pub;
ros::Publisher _thrust_sp_pub;
};