diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:28:51 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2015-02-28 18:28:51 +0100 |
commit | b4d7bac8c25d6b3b5cf277900295fac0c8384755 (patch) | |
tree | 0e32c46554f76f73132568bfe5897b4bdfa32de6 /src | |
parent | 1df0f25c6b112ef568f7a19a7819a8e7948d8515 (diff) | |
parent | 482f2c94424f32e45aaee68b8b515e1eab40b6de (diff) | |
download | px4-firmware-b4d7bac8c25d6b3b5cf277900295fac0c8384755.tar.gz px4-firmware-b4d7bac8c25d6b3b5cf277900295fac0c8384755.tar.bz2 px4-firmware-b4d7bac8c25d6b3b5cf277900295fac0c8384755.zip |
Merge pull request #1793 from PX4/ros_mavlink
ROS SITL: offboard setpoints
Diffstat (limited to 'src')
-rw-r--r-- | src/platforms/px4_includes.h | 4 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.cpp | 77 | ||||
-rw-r--r-- | src/platforms/ros/nodes/commander/commander.h | 16 | ||||
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp | 85 | ||||
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h (renamed from src/modules/uORB/topics/offboard_control_mode.h) | 67 | ||||
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp | 77 | ||||
-rw-r--r-- | src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h | 57 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.cpp | 17 | ||||
-rw-r--r-- | src/platforms/ros/nodes/manual_input/manual_input.h | 2 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.cpp | 296 | ||||
-rw-r--r-- | src/platforms/ros/nodes/mavlink/mavlink.h | 113 |
11 files changed, 767 insertions, 44 deletions
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index f8561fa3b..0e98783fd 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -66,6 +66,8 @@ #include <px4_vehicle_global_velocity_setpoint.h> #include <px4_vehicle_local_position.h> #include <px4_position_setpoint_triplet.h> +#include <px4_offboard_control_mode.h> +#include <px4_vehicle_force_setpoint.h> #endif #else @@ -93,6 +95,8 @@ #include <platforms/nuttx/px4_messages/px4_vehicle_global_velocity_setpoint.h> #include <platforms/nuttx/px4_messages/px4_vehicle_local_position.h> #include <platforms/nuttx/px4_messages/px4_position_setpoint_triplet.h> +#include <platforms/nuttx/px4_messages/px4_offboard_control_mode.h> +#include <platforms/nuttx/px4_messages/px4_vehicle_force_setpoint.h> #endif #include <systemlib/err.h> #include <systemlib/param/param.h> diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 2673122c7..0c32026f3 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -45,18 +45,25 @@ Commander::Commander() : _n(), _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)), + _offboard_control_mode_sub(_n.subscribe("offboard_control_mode", 10, &Commander::OffboardControlModeCallback, this)), _vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)), _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)), _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)), _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)), _msg_parameter_update(), _msg_actuator_armed(), - _msg_vehicle_control_mode() + _msg_vehicle_control_mode(), + _msg_offboard_control_mode(), + _got_manual_control(false) { + + /* Default to offboard control: when no joystick is connected offboard control should just work */ + } void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg) { + _got_manual_control = true; px4::vehicle_status msg_vehicle_status; /* fill vehicle control mode based on (faked) stick positions*/ @@ -101,12 +108,51 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } } +void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ + msg_vehicle_control_mode.flag_control_manual_enabled = false; + msg_vehicle_control_mode.flag_control_offboard_enabled = true; + msg_vehicle_control_mode.flag_control_auto_enabled = false; + + msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; + + + msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; + + msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_position; +} + void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode) { // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander + + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) + { + SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); + return; + } + + msg_vehicle_control_mode.flag_control_offboard_enabled = false; + switch (msg->mode_switch) { case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); @@ -152,6 +198,35 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, } +void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg) +{ + _msg_offboard_control_mode = *msg; + + /* Force system into offboard control mode */ + if (!_got_manual_control) { + SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); + + px4::vehicle_status msg_vehicle_status; + msg_vehicle_status.timestamp = px4::get_time_micros(); + msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF; + msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; + msg_vehicle_status.is_rotary_wing = true; + msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED; + + + _msg_actuator_armed.armed = true; + _msg_actuator_armed.timestamp = px4::get_time_micros(); + + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + _msg_vehicle_control_mode.flag_armed = true; + + + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + _actuator_armed_pub.publish(_msg_actuator_armed); + _vehicle_status_pub.publish(msg_vehicle_status); + } +} + int main(int argc, char **argv) { ros::init(argc, argv, "commander"); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 58b7257b7..c537c8419 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -44,6 +44,7 @@ #include <px4/vehicle_status.h> #include <px4/parameter_update.h> #include <px4/actuator_armed.h> +#include <px4/offboard_control_mode.h> class Commander { @@ -59,14 +60,26 @@ protected: void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg); /** + * Stores the offboard control mode + */ + void OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg); + + /** * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, px4::vehicle_status &msg_vehicle_status, px4::vehicle_control_mode &msg_vehicle_control_mode); + /** + * Sets offboard controll flags in msg_vehicle_control_mode + */ + void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, + px4::vehicle_control_mode &msg_vehicle_control_mode); + ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; + ros::Subscriber _offboard_control_mode_sub; ros::Publisher _vehicle_control_mode_pub; ros::Publisher _actuator_armed_pub; ros::Publisher _vehicle_status_pub; @@ -75,5 +88,8 @@ protected: px4::parameter_update _msg_parameter_update; px4::actuator_armed _msg_actuator_armed; px4::vehicle_control_mode _msg_vehicle_control_mode; + px4::offboard_control_mode _msg_offboard_control_mode; + + bool _got_manual_control; }; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp new file mode 100644 index 000000000..fb0b09de1 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * 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> +#include <math.h> +#include <tf/transform_datatypes.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; + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + quaternionTFToMsg(q, pose.pose.orientation); + + _attitude_sp_pub.publish(pose); + + std_msgs::Float64 thrust; + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + _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(); +} diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h index 559659a1d..d7b7a37ba 100644 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h @@ -1,21 +1,20 @@ /**************************************************************************** * - * Copyright (C) 2008-2015 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * 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. + * 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. + * 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. + * 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 @@ -33,41 +32,27 @@ ****************************************************************************/ /** - * @file offboard_control_mode.h - * Definition of the manual_control_setpoint uORB topic. - */ - -#ifndef TOPIC_OFFBOARD_CONTROL_MODE_H_ -#define TOPIC_OFFBOARD_CONTROL_MODE_H_ - -#include <stdint.h> -#include "../uORB.h" - -/** - * Off-board control mode - */ - -/** - * @addtogroup topics - * @{ - */ + * @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> +*/ -struct offboard_control_mode_s { - uint64_t timestamp; +#include "ros/ros.h" +#include <px4/manual_control_setpoint.h> - bool ignore_thrust; - bool ignore_attitude; - bool ignore_bodyrate; - bool ignore_position; - bool ignore_velocity; - bool ignore_acceleration_force; +class DemoOffboardAttitudeSetpoints +{ +public: + DemoOffboardAttitudeSetpoints(); -}; /**< offboard control inputs */ -/** - * @} - */ + ~DemoOffboardAttitudeSetpoints() {} -/* register this as object request broker structure */ -ORB_DECLARE(offboard_control_mode); + int main(); -#endif +protected: + ros::NodeHandle _n; + ros::Publisher _attitude_sp_pub; + ros::Publisher _thrust_sp_pub; +}; diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp new file mode 100644 index 000000000..7366d7fc6 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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_position_setpoints.h" + +#include <platforms/px4_middleware.h> +#include <geometry_msgs/PoseStamped.h> + +DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : + _n(), + _local_position_sp_pub(_n.advertise<geometry_msgs::PoseStamped>("mavros/setpoint/local_position", 1)) +{ +} + + +int DemoOffboardPositionSetpoints::main() +{ + px4::Rate loop_rate(10); + + while (ros::ok()) { + loop_rate.sleep(); + ros::spinOnce(); + + /* Publish example offboard position setpoint */ + geometry_msgs::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.position.z = 1; + _local_position_sp_pub.publish(pose); + } + return 0; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "demo_offboard_position_setpoints"); + DemoOffboardPositionSetpoints d; + return d.main(); +} diff --git a/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h new file mode 100644 index 000000000..7d39690f4 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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.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 DemoOffboardPositionSetpoints +{ +public: + DemoOffboardPositionSetpoints(); + + ~DemoOffboardPositionSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _local_position_sp_pub; +}; diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 72f6e252f..8488c518f 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -76,7 +76,8 @@ ManualInput::ManualInput() : _n.param("map_posctl", _param_buttons_map[2], 2); _n.param("map_auto_mission", _param_buttons_map[3], 3); _n.param("map_auto_loiter", _param_buttons_map[4], 4); - _n.param("map_auto_rtl", _param_buttons_map[5], 4); + _n.param("map_auto_rtl", _param_buttons_map[5], 5); + _n.param("map_offboard", _param_buttons_map[6], 6); /* Default to manual */ _msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -84,6 +85,8 @@ ManualInput::ManualInput() : _msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; _msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + _msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; } @@ -120,7 +123,6 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; - msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; @@ -128,6 +130,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_ALTCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_ALTCTL]] == true) { @@ -136,6 +139,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; } else if (_buttons_state[MAIN_STATE_POSCTL] != msg->buttons[_param_buttons_map[MAIN_STATE_POSCTL]] == true) { @@ -144,6 +148,15 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { + msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.posctl_switch = px4::manual_control_setpoint::SWITCH_POS_ON; + msg_mc_sp.loiter_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; + msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_ON; return; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h index bf704f675..2bafcca2e 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.h +++ b/src/platforms/ros/nodes/manual_input/manual_input.h @@ -77,6 +77,8 @@ protected: MAIN_STATE_AUTO_MISSION, MAIN_STATE_AUTO_LOITER, MAIN_STATE_AUTO_RTL, + // MAIN_STATE_ACRO, + MAIN_STATE_OFFBOARD, MAIN_STATE_MAX }; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp new file mode 100644 index 000000000..5459fcffd --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -0,0 +1,296 @@ +/**************************************************************************** + * + * 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 mavlink.cpp + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "mavlink.h" + +#include <platforms/px4_middleware.h> + +using namespace px4; + +Mavlink::Mavlink() : + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_local_pos_sub(_n.subscribe("vehicle_local_position", 1, &Mavlink::VehicleLocalPositionCallback, this)), + _v_att_sp_pub(_n.advertise<vehicle_attitude_setpoint>("vehicle_attitude_setpoint", 1)), + _pos_sp_triplet_pub(_n.advertise<position_setpoint_triplet>("position_setpoint_triplet", 1)), + _offboard_control_mode_pub(_n.advertise<offboard_control_mode>("offboard_control_mode", 1)), + _force_sp_pub(_n.advertise<vehicle_force_setpoint>("vehicle_force_setpoint", 1)) +{ + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); + _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); + +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mavlink"); + Mavlink m; + ros::spin(); + return 0; +} + +void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_attitude_quaternion_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); + _link->send_message(&msg_m); +} + +void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg) +{ + mavlink_message_t msg_m; + mavlink_msg_local_position_ned_pack_chan( + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); + _link->send_message(&msg_m); +} + +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { + (void)sysid; + (void)compid; + + switch(mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + default: + break; + } + +} + +void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) +{ + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); + + static offboard_control_mode offboard_control_mode; + + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_mode.ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6)); + + /* + * if attitude or body rate have been used (not ignored) previously and this message only sends + * throttle and has the ignore bits set for attitude and rates don't change the flags for attitude and + * body rates to keep the controllers running + */ + bool ignore_bodyrate = (bool)(set_attitude_target.type_mask & 0x7); + bool ignore_attitude = (bool)(set_attitude_target.type_mask & (1 << 7)); + + if (ignore_bodyrate && ignore_attitude && !offboard_control_mode.ignore_thrust) { + /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ + offboard_control_mode.ignore_bodyrate = ignore_bodyrate && offboard_control_mode.ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude && offboard_control_mode.ignore_attitude; + } else { + offboard_control_mode.ignore_bodyrate = ignore_bodyrate; + offboard_control_mode.ignore_attitude = ignore_attitude; + } + offboard_control_mode.ignore_position = true; + offboard_control_mode.ignore_velocity = true; + offboard_control_mode.ignore_acceleration_force = true; + + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + static vehicle_attitude_setpoint att_sp = {}; + + /* The real mavlink app has a ckeck at this location which makes sure that the attitude setpoint + * gets published only if in offboard mode. We leave that out for now. + */ + + att_sp.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + + if (!offboard_control_mode.ignore_thrust) { + att_sp.thrust = set_attitude_target.thrust; + } + + if (!offboard_control_mode.ignore_attitude) { + for (ssize_t i = 0; i < 4; i++) { + att_sp.q_d[i] = set_attitude_target.q[i]; + } + att_sp.q_d_valid = true; + } + + _v_att_sp_pub.publish(att_sp); + + + //XXX real mavlink publishes rate sp here + +} + +void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg) +{ + + mavlink_set_position_target_local_ned_t set_position_target_local_ned; + mavlink_msg_set_position_target_local_ned_decode(mmsg, &set_position_target_local_ned); + + offboard_control_mode offboard_control_mode; + // memset(&offboard_control_mode, 0, sizeof(offboard_control_mode));//XXX breaks compatibility with multiple setpoints + + /* Only accept messages which are intended for this system */ + // XXX removed for sitl, makes maybe sense to re-introduce at some point + // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); + offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38); + offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0); + bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); + /* yaw ignore flag mapps to ignore_attitude */ + offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400); + /* yawrate ignore flag mapps to ignore_bodyrate */ + offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800); + + + + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); + + /* The real mavlink app has a ckeck at this location which makes sure that the position setpoint triplet + * gets published only if in offboard mode. We leave that out for now. + */ + if (is_force_sp && offboard_control_mode.ignore_position && + offboard_control_mode.ignore_velocity) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ + vehicle_force_setpoint force_sp; + force_sp.x = set_position_target_local_ned.afx; + force_sp.y = set_position_target_local_ned.afy; + force_sp.z = set_position_target_local_ned.afz; + //XXX: yaw + + _force_sp_pub.publish(force_sp); + } else { + /* It's not a pure force setpoint: publish to setpoint triplet topic */ + position_setpoint_triplet pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = position_setpoint::SETPOINT_TYPE_POSITION; //XXX support others + + /* set the local pos values */ + if (!offboard_control_mode.ignore_position) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = set_position_target_local_ned.x; + pos_sp_triplet.current.y = set_position_target_local_ned.y; + pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { + pos_sp_triplet.current.position_valid = false; + } + + /* set the local vel values */ + if (!offboard_control_mode.ignore_velocity) { + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = set_position_target_local_ned.vx; + pos_sp_triplet.current.vy = set_position_target_local_ned.vy; + pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { + pos_sp_triplet.current.velocity_valid = false; + } + + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (!offboard_control_mode.ignore_acceleration_force) { + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = set_position_target_local_ned.afx; + pos_sp_triplet.current.a_y = set_position_target_local_ned.afy; + pos_sp_triplet.current.a_z = set_position_target_local_ned.afz; + pos_sp_triplet.current.acceleration_is_force = + is_force_sp; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } + + /* set the yaw sp value */ + if (!offboard_control_mode.ignore_attitude) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value */ + if (!offboard_control_mode.ignore_bodyrate) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } + //XXX handle global pos setpoints (different MAV frames) + + _pos_sp_triplet_pub.publish(pos_sp_triplet); + } +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h new file mode 100644 index 000000000..acb2408f3 --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 mavlink.h + * Dummy mavlink node that interfaces to a mavros node via UDP + * This simulates the onboard mavlink app to some degree. It should be possible to + * send offboard setpoints via mavros to the SITL setup the same way as on the real system + * + * @author Thomas Gubler <thomasgubler@gmail.com> +*/ + +#include "ros/ros.h" +#include <mavconn/interface.h> +#include <px4/vehicle_attitude.h> +#include <px4/vehicle_local_position.h> +#include <px4/vehicle_attitude_setpoint.h> +#include <px4/position_setpoint_triplet.h> +#include <px4/vehicle_force_setpoint.h> +#include <px4/offboard_control_mode.h> + +namespace px4 +{ + +class Mavlink +{ +public: + Mavlink(); + + ~Mavlink() {} + +protected: + + ros::NodeHandle _n; + mavconn::MAVConnInterface::Ptr _link; + ros::Subscriber _v_att_sub; + ros::Subscriber _v_local_pos_sub; + ros::Publisher _v_att_sp_pub; + ros::Publisher _pos_sp_triplet_pub; + ros::Publisher _offboard_control_mode_pub; + ros::Publisher _force_sp_pub; + + /** + * + * Simulates output of attitude data from the FCU + * Equivalent to the mavlink stream ATTITUDE_QUATERNION + * + * */ + void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg); + + /** + * + * Simulates output of local position data from the FCU + * Equivalent to the mavlink stream LOCAL_POSITION_NED + * + * */ + void VehicleLocalPositionCallback(const vehicle_local_positionConstPtr &msg); + + + /** + * + * Handle incoming mavlink messages ant publish them to ROS ("Mavlink Receiver") + * + * */ + void handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid); + + /** + * + * Handle SET_ATTITUDE_TARGET mavlink messages + * + * */ + void handle_msg_set_attitude_target(const mavlink_message_t *mmsg); + + /** + * + * Handle SET_POSITION_TARGET_LOCAL_NED mavlink messages + * + * */ + void handle_msg_set_position_target_local_ned(const mavlink_message_t *mmsg); + +}; + +} |