From 2d0c5616cbc89f0621e2e8eb56b06635ecedfd90 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Feb 2015 22:35:32 +0100 Subject: [PATCH 01/37] ros: add skeleton for mavlink node --- CMakeLists.txt | 9 ++++ src/platforms/ros/nodes/mavlink/mavlink.cpp | 60 +++++++++++++++++++++ src/platforms/ros/nodes/mavlink/mavlink.h | 60 +++++++++++++++++++++ 3 files changed, 129 insertions(+) create mode 100644 src/platforms/ros/nodes/mavlink/mavlink.cpp create mode 100644 src/platforms/ros/nodes/mavlink/mavlink.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c81607b00..1434b43961 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -240,6 +240,15 @@ target_link_libraries(commander px4 ) +## Mavlink +add_executable(mavlink + src/platforms/ros/nodes/mavlink/mavlink.cpp) +add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(mavlink + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp new file mode 100644 index 0000000000..e64e576bde --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 +*/ + +#include "mavlink.h" + +#include + +using namespace px4; + +Mavlink::Mavlink() : + _n() +{ +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "mavlink"); + Mavlink m; + ros::spin(); + return 0; +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h new file mode 100644 index 0000000000..2723df35c4 --- /dev/null +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 +*/ + +#include "ros/ros.h" + +namespace px4 +{ + +class Mavlink +{ +public: + Mavlink(); + + ~Mavlink() {} + +protected: + + ros::NodeHandle _n; +}; + +} From 001575e740261acf9de68023ab8e8bd59a478ce3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 10 Feb 2015 23:34:26 +0100 Subject: [PATCH 02/37] ros: mavlink node: add mavconn link --- CMakeLists.txt | 4 +++- package.xml | 2 ++ src/platforms/ros/nodes/mavlink/mavlink.cpp | 2 ++ src/platforms/ros/nodes/mavlink/mavlink.h | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1434b43961..491c2c7ed2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,7 @@ project(px4) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") add_definitions(-D__PX4_ROS) add_definitions(-D__EXPORT=) +add_definitions(-DMAVLINK_DIALECT=common) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -16,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS gazebo_msgs sensor_msgs mav_msgs + libmavconn ) find_package(Eigen REQUIRED) @@ -109,7 +111,7 @@ generate_messages( catkin_package( INCLUDE_DIRS src/include LIBRARIES px4 - CATKIN_DEPENDS message_runtime roscpp rospy std_msgs + CATKIN_DEPENDS message_runtime roscpp rospy std_msgs libmavconn DEPENDS system_lib ) diff --git a/package.xml b/package.xml index 6662003907..96d622a682 100644 --- a/package.xml +++ b/package.xml @@ -44,10 +44,12 @@ rospy std_msgs eigen + libmavconn roscpp rospy std_msgs eigen + libmavconn diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index e64e576bde..b6413c3b4e 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,6 +49,8 @@ using namespace px4; Mavlink::Mavlink() : _n() { + + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 2723df35c4..5b39468870 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -41,6 +41,7 @@ */ #include "ros/ros.h" +#include namespace px4 { @@ -55,6 +56,7 @@ public: protected: ros::NodeHandle _n; + mavconn::MAVConnInterface::Ptr _link; }; } From 3e5cbfcf77939d5f650885c7a82aaf527d40a094 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 11 Feb 2015 23:02:58 +0100 Subject: [PATCH 03/37] ros: mavlink onboard node: send attitude via mavlink --- NuttX | 2 +- launch/multicopter.launch | 1 + src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 +++++++++++++++++++-- src/platforms/ros/nodes/mavlink/mavlink.h | 4 ++++ 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/NuttX b/NuttX index 11afcdfee6..787aca971a 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 11afcdfee6a3961952dd92f02c1abaa4756b115f +Subproject commit 787aca971a86219d4e791100646b54ed8245a733 diff --git a/launch/multicopter.launch b/launch/multicopter.launch index 95400bd82d..bc0e377715 100644 --- a/launch/multicopter.launch +++ b/launch/multicopter.launch @@ -9,6 +9,7 @@ + diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index b6413c3b4e..131a4930f2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -47,10 +47,11 @@ using namespace px4; Mavlink::Mavlink() : - _n() + _n(), + _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) { - _link = mavconn::MAVConnInterface::open_url("udp://localhost:14551@localhost:14552"); + _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); } int main(int argc, char **argv) @@ -60,3 +61,22 @@ int main(int argc, char **argv) ros::spin(); return 0; } + +void Mavlink::VehicleAttitudeCallback(const px4::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, //XXX hardcoded + 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); +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 5b39468870..e683597a94 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -42,6 +42,7 @@ #include "ros/ros.h" #include +#include namespace px4 { @@ -57,6 +58,9 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; + ros::Subscriber _v_att_sub; + + void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } From 93f8fc33c890bb961f0fba03537cc54bf8a88d1f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 12 Feb 2015 20:43:52 +0100 Subject: [PATCH 04/37] ros mavlink node: handle set_attitude_target --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 43 +++++++++++++++++++-- src/platforms/ros/nodes/mavlink/mavlink.h | 26 ++++++++++++- 2 files changed, 65 insertions(+), 4 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 131a4930f2..a7830e97fb 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -48,10 +48,12 @@ using namespace px4; Mavlink::Mavlink() : _n(), - _v_att_sub(_n.subscribe("vehicle_attitude", 10, &Mavlink::VehicleAttitudeCallback, this)) + _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), + _v_att_sp_pub(_n.advertise("vehicle_attitude_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) @@ -62,7 +64,7 @@ int main(int argc, char **argv) return 0; } -void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) +void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( @@ -80,3 +82,38 @@ void Mavlink::VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg) msg->yawspeed); _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; + default: + break; + } + +} + +void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) +{ + mavlink_set_attitude_target_t set_att_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); + + vehicle_attitude_setpoint msg; + + msg.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &msg.roll_body, &msg.pitch_body, &msg.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])msg.R_body.data()); + msg.R_valid = true; + msg.thrust = set_att_target.thrust; + for (ssize_t i = 0; i < 4; i++) { + msg.q_d[i] = set_att_target.q[i]; + } + msg.q_d_valid = true; + + _v_att_sp_pub.publish(msg); + +} diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index e683597a94..6f75364363 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include #include +#include namespace px4 { @@ -59,8 +60,31 @@ protected: ros::NodeHandle _n; mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; + ros::Publisher _v_att_sp_pub; + + /** + * + * Simulates output of attitude data from the FCU + * Equivalent to the mavlink stream ATTITUDE + * + * */ + void VehicleAttitudeCallback(const vehicle_attitudeConstPtr &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); - void VehicleAttitudeCallback(const px4::vehicle_attitudeConstPtr &msg); }; } From 1c9509c235fe9b9d36612768a9935163c7b18cfa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:45:05 +0100 Subject: [PATCH 05/37] move offboard_control_mode topic to msg mode --- msg/offboard_control_mode.msg | 9 +++ .../uORB/topics/offboard_control_mode.h | 73 ------------------- 2 files changed, 9 insertions(+), 73 deletions(-) create mode 100644 msg/offboard_control_mode.msg delete mode 100644 src/modules/uORB/topics/offboard_control_mode.h diff --git a/msg/offboard_control_mode.msg b/msg/offboard_control_mode.msg new file mode 100644 index 0000000000..e2d06963b6 --- /dev/null +++ b/msg/offboard_control_mode.msg @@ -0,0 +1,9 @@ +# Off-board control mode +uint64 timestamp + +bool ignore_thrust +bool ignore_attitude +bool ignore_bodyrate +bool ignore_position +bool ignore_velocity +bool ignore_acceleration_force diff --git a/src/modules/uORB/topics/offboard_control_mode.h b/src/modules/uORB/topics/offboard_control_mode.h deleted file mode 100644 index 559659a1d4..0000000000 --- a/src/modules/uORB/topics/offboard_control_mode.h +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2015 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 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 -#include "../uORB.h" - -/** - * Off-board control mode - */ - -/** - * @addtogroup topics - * @{ - */ - -struct offboard_control_mode_s { - uint64_t timestamp; - - bool ignore_thrust; - bool ignore_attitude; - bool ignore_bodyrate; - bool ignore_position; - bool ignore_velocity; - bool ignore_acceleration_force; - -}; /**< offboard control inputs */ -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(offboard_control_mode); - -#endif From 3475d8883be075b8fb1e476154a88870ca2bd5e5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:45:46 +0100 Subject: [PATCH 06/37] enable offboard control mode topic for multiplatform --- CMakeLists.txt | 1 + src/platforms/px4_includes.h | 2 ++ 2 files changed, 3 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 491c2c7ed2..4e91fb3bee 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ add_message_files( position_setpoint_triplet.msg vehicle_local_position_setpoint.msg vehicle_global_velocity_setpoint.msg + offboard_control_mode.msg ) ## Generate services in the 'srv' folder diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index f8561fa3b7..364a5f31b9 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -66,6 +66,7 @@ #include #include #include +#include #endif #else @@ -93,6 +94,7 @@ #include #include #include +#include #endif #include #include From 4869f9f3d46babb53cdde47fabda4d1b22399565 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:47:24 +0100 Subject: [PATCH 07/37] ros:manual input dummy node: add offboard button --- .../ros/nodes/manual_input/manual_input.cpp | 15 +++++++++++++-- .../ros/nodes/manual_input/manual_input.h | 2 ++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 72f6e252f9..d5d14a21fb 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; @@ -120,7 +121,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 +128,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 +137,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 +146,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 bf704f6757..2bafcca2e8 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 }; From 01b8a18ad520a9d7bfecd3eea9a2e1dfc76b0ab1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:48:25 +0100 Subject: [PATCH 08/37] ros: mavlink dummy node: parse attitude target messages --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 36 +++++++++++++++------ src/platforms/ros/nodes/mavlink/mavlink.h | 2 ++ 2 files changed, 28 insertions(+), 10 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index a7830e97fb..8d658caa57 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,7 +49,7 @@ using namespace px4; Mavlink::Mavlink() : _n(), _v_att_sub(_n.subscribe("vehicle_attitude", 1, &Mavlink::VehicleAttitudeCallback, this)), - _v_att_sp_pub(_n.advertise("vehicle_attitude_setpoint", 1)) + _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); _link->message_received.connect(boost::bind(&Mavlink::handle_msg, this, _1, _2, _3)); @@ -102,18 +102,34 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_att_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); - vehicle_attitude_setpoint msg; + offboard_control_mode offboard_control_mode_msg; + /* set correct ignore flags for body rate fields: copy from mavlink message */ + offboard_control_mode_msg.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_mode_msg.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_mode_msg.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); - msg.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &msg.roll_body, &msg.pitch_body, &msg.yaw_body); - mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])msg.R_body.data()); - msg.R_valid = true; - msg.thrust = set_att_target.thrust; + offboard_control_mode_msg.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode_msg); + + vehicle_attitude_setpoint v_att_sp_msg; + + /* 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. + * */ + + v_att_sp_msg.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &v_att_sp_msg.roll_body, &v_att_sp_msg.pitch_body, + &v_att_sp_msg.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])v_att_sp_msg.R_body.data()); + v_att_sp_msg.R_valid = true; + v_att_sp_msg.thrust = set_att_target.thrust; for (ssize_t i = 0; i < 4; i++) { - msg.q_d[i] = set_att_target.q[i]; + v_att_sp_msg.q_d[i] = set_att_target.q[i]; } - msg.q_d_valid = true; + v_att_sp_msg.q_d_valid = true; - _v_att_sp_pub.publish(msg); + _v_att_sp_pub.publish(v_att_sp_msg); } diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 6f75364363..2ee383b4f2 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace px4 { @@ -61,6 +62,7 @@ protected: mavconn::MAVConnInterface::Ptr _link; ros::Subscriber _v_att_sub; ros::Publisher _v_att_sp_pub; + ros::Publisher _offboard_control_mode_pub; /** * From 582c664a9c61e3b6cb4762e90ce437e5843c5d14 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 16:49:06 +0100 Subject: [PATCH 09/37] ros: commander dummy node: set control flags in offboard mode --- .../ros/nodes/commander/commander.cpp | 36 ++++++++++++++++++- src/platforms/ros/nodes/commander/commander.h | 8 +++++ 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 2673122c70..b0f905d23f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -45,13 +45,15 @@ 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("vehicle_control_mode", 10)), _actuator_armed_pub(_n.advertise("actuator_armed", 10)), _vehicle_status_pub(_n.advertise("vehicle_status", 10)), _parameter_update_pub(_n.advertise("parameter_update", 10)), _msg_parameter_update(), _msg_actuator_armed(), - _msg_vehicle_control_mode() + _msg_vehicle_control_mode(), + _msg_offboard_control_mode() { } @@ -107,6 +109,33 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander + + if (msg->offboard_switch) + { + 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; + return; + } + switch (msg->mode_switch) { case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); @@ -152,6 +181,11 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, } +void Commander::OffboardControlModeCallback(const px4::offboard_control_modeConstPtr &msg) +{ + _msg_offboard_control_mode = *msg; +} + 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 58b7257b72..3152055aee 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -44,6 +44,7 @@ #include #include #include +#include class Commander { @@ -58,6 +59,11 @@ 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) */ @@ -67,6 +73,7 @@ protected: 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 +82,6 @@ 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; }; From 8d36305f8b5d9393003f6074327ba279c98622ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Feb 2015 18:05:43 +0100 Subject: [PATCH 10/37] add mavros sitl launch file --- launch/mavros_sitl.launch | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 launch/mavros_sitl.launch diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch new file mode 100644 index 0000000000..17b6a7035b --- /dev/null +++ b/launch/mavros_sitl.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + From ca250d21eb13b9887773422e63ff664447dfe264 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 11:40:07 +0100 Subject: [PATCH 11/37] ros: mavlink dummy node: listen to vehicle local position and publish to mavlink (LOCAL_POSITION_NED) --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 21 ++++++++++++++++++++- src/platforms/ros/nodes/mavlink/mavlink.h | 12 +++++++++++- 2 files changed, 31 insertions(+), 2 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 8d658caa57..3485b1f4ec 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -49,6 +49,7 @@ 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)), _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) { _link = mavconn::MAVConnInterface::open_url("udp://localhost:14565@localhost:14560"); @@ -71,7 +72,7 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _link->get_system_id(), _link->get_component_id(), _link->get_channel(), - &msg_m, //XXX hardcoded + &msg_m, get_time_micros() / 1000, msg->q[0], msg->q[1], @@ -83,6 +84,24 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) _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; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.h b/src/platforms/ros/nodes/mavlink/mavlink.h index 2ee383b4f2..a246af4a4b 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -43,6 +43,7 @@ #include "ros/ros.h" #include #include +#include #include #include @@ -61,17 +62,26 @@ 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 _offboard_control_mode_pub; /** * * Simulates output of attitude data from the FCU - * Equivalent to the mavlink stream ATTITUDE + * 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); + /** * From 6e69558b42243a2b661d6fc48fc07a22961d4e9e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 12:39:32 +0100 Subject: [PATCH 12/37] enable force setpoint message for multiplatform --- CMakeLists.txt | 1 + msg/vehicle_force_setpoint.msg | 8 ++++++++ src/platforms/px4_includes.h | 2 ++ 3 files changed, 11 insertions(+) create mode 100644 msg/vehicle_force_setpoint.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e91fb3bee..c3e8943886 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -77,6 +77,7 @@ add_message_files( vehicle_local_position_setpoint.msg vehicle_global_velocity_setpoint.msg offboard_control_mode.msg + vehicle_force_setpoint.msg ) ## Generate services in the 'srv' folder diff --git a/msg/vehicle_force_setpoint.msg b/msg/vehicle_force_setpoint.msg new file mode 100644 index 0000000000..9e2322005d --- /dev/null +++ b/msg/vehicle_force_setpoint.msg @@ -0,0 +1,8 @@ +# Definition of force (NED) setpoint uORB topic. Typically this can be used +# by a position control app together with an attitude control app. + + +float32 x # in N NED +float32 y # in N NED +float32 z # in N NED +float32 yaw # right-hand rotation around downward axis (rad, equivalent to Tait-Bryan yaw) diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index 364a5f31b9..0e98783fda 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -67,6 +67,7 @@ #include #include #include +#include #endif #else @@ -95,6 +96,7 @@ #include #include #include +#include #endif #include #include From 5beafd25e6947b5f6ac33fe66521fb462a1be1b0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 12:40:46 +0100 Subject: [PATCH 13/37] ros: mavlink dummy node: handle position target local ned mavlink messages and forward them to position_setpoint_triplet --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 149 +++++++++++++++++--- src/platforms/ros/nodes/mavlink/mavlink.h | 11 ++ 2 files changed, 142 insertions(+), 18 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 3485b1f4ec..dbb3dac7af 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -50,7 +50,10 @@ 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)), - _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)) + _v_att_sp_pub(_n.advertise("vehicle_attitude_setpoint", 1)), + _pos_sp_triplet_pub(_n.advertise("position_setpoint_triplet", 1)), + _offboard_control_mode_pub(_n.advertise("offboard_control_mode", 1)), + _force_sp_pub(_n.advertise("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)); @@ -121,34 +124,144 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) mavlink_set_attitude_target_t set_att_target; mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); - offboard_control_mode offboard_control_mode_msg; + offboard_control_mode offboard_control_mode; /* set correct ignore flags for body rate fields: copy from mavlink message */ - offboard_control_mode_msg.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + offboard_control_mode.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode_msg.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); + offboard_control_mode.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode_msg.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); + offboard_control_mode.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); - offboard_control_mode_msg.timestamp = get_time_micros(); - _offboard_control_mode_pub.publish(offboard_control_mode_msg); + offboard_control_mode.timestamp = get_time_micros(); + _offboard_control_mode_pub.publish(offboard_control_mode); - vehicle_attitude_setpoint v_att_sp_msg; + 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. - * */ + */ - v_att_sp_msg.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &v_att_sp_msg.roll_body, &v_att_sp_msg.pitch_body, - &v_att_sp_msg.yaw_body); - mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])v_att_sp_msg.R_body.data()); - v_att_sp_msg.R_valid = true; - v_att_sp_msg.thrust = set_att_target.thrust; + att_sp.timestamp = get_time_micros(); + mavlink_quaternion_to_euler(set_att_target.q, &att_sp.roll_body, &att_sp.pitch_body, + &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_att_target.q, (float(*)[3])att_sp.R_body.data()); + att_sp.R_valid = true; + att_sp.thrust = set_att_target.thrust; for (ssize_t i = 0; i < 4; i++) { - v_att_sp_msg.q_d[i] = set_att_target.q[i]; + att_sp.q_d[i] = set_att_target.q[i]; } - v_att_sp_msg.q_d_valid = true; + att_sp.q_d_valid = true; - _v_att_sp_pub.publish(v_att_sp_msg); + _v_att_sp_pub.publish(att_sp); } + +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 index a246af4a4b..acb2408f30 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.h +++ b/src/platforms/ros/nodes/mavlink/mavlink.h @@ -45,6 +45,8 @@ #include #include #include +#include +#include #include namespace px4 @@ -64,7 +66,9 @@ protected: 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; /** * @@ -97,6 +101,13 @@ protected: * */ 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); + }; } From ae64e4e05c2a8f6d179f817c9d591881aafcfee6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:29:57 +0100 Subject: [PATCH 14/37] ros: manual input (joystick) node: correctly initilize switches --- src/platforms/ros/nodes/manual_input/manual_input.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index d5d14a21fb..8488c518f5 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -85,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; } From 8b40112e9f6ad25a41cdef73f3d3001be49c0271 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:31:49 +0100 Subject: [PATCH 15/37] ros: commander dummy node: fix offboard support --- .../ros/nodes/commander/commander.cpp | 70 +++++++++++++------ src/platforms/ros/nodes/commander/commander.h | 8 +++ 2 files changed, 55 insertions(+), 23 deletions(-) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index b0f905d23f..9ca54339df 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -53,12 +53,17 @@ Commander::Commander() : _msg_parameter_update(), _msg_actuator_armed(), _msg_vehicle_control_mode(), - _msg_offboard_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*/ @@ -103,6 +108,36 @@ 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) { @@ -110,32 +145,14 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, // needed consider a full port of the commander - if (msg->offboard_switch) + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { - 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; + 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"); @@ -184,6 +201,13 @@ 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); + _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); + } } int main(int argc, char **argv) diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 3152055aee..c537c84193 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -71,6 +71,12 @@ protected: 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; @@ -84,4 +90,6 @@ protected: px4::vehicle_control_mode _msg_vehicle_control_mode; px4::offboard_control_mode _msg_offboard_control_mode; + bool _got_manual_control; + }; From b829b553f4a3dcf2357e482dc8b1c868c779e29d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:33:17 +0100 Subject: [PATCH 16/37] ros: mavlink dummy node: actually call handle_msg_set_position_target_local_ned --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index dbb3dac7af..013788ecd8 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -113,6 +113,9 @@ void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t c 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; } From 1a6cbe170cba5e90a9082df0d46a2de39857f967 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:33:55 +0100 Subject: [PATCH 17/37] ros: demo node for offboard position setpoints --- .../demo_offboard_position_setpoints.cpp | 77 +++++++++++++++++++ .../demo_offboard_position_setpoints.h | 57 ++++++++++++++ 2 files changed, 134 insertions(+) create mode 100644 src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp create mode 100644 src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h 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 0000000000..c7dc358dd5 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 +*/ + +#include "demo_offboard_position_setpoints.h" + +#include +#include + +DemoOffboardPositionSetpoints::DemoOffboardPositionSetpoints() : + _n(), + _local_position_sp_pub(_n.advertise("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 0000000000..14187a9cf8 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 +*/ + +#include "ros/ros.h" +#include + +class DemoOffboardPositionSetpoints +{ +public: + DemoOffboardPositionSetpoints(); + + ~DemoOffboardPositionSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _local_position_sp_pub; +}; From 39a105df73efb193009273e40ae3ab21d8deb7ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 18:34:27 +0100 Subject: [PATCH 18/37] ros: launch files for offboard postion setpoints demo --- ...o_ardrone_empty_world_offboard_demo.launch | 10 +++++++ launch/mavros_sitl.launch | 26 ++++++++++--------- 2 files changed, 24 insertions(+), 12 deletions(-) create mode 100644 launch/gazebo_ardrone_empty_world_offboard_demo.launch diff --git a/launch/gazebo_ardrone_empty_world_offboard_demo.launch b/launch/gazebo_ardrone_empty_world_offboard_demo.launch new file mode 100644 index 0000000000..ce2386920f --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_demo.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 17b6a7035b..29f7dd857f 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -2,18 +2,20 @@ - - - - + + + + + - - - + + + - - - - - + + + + + + From dae7c698b0356643b15fafe8553d84b2084328f2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 19:05:47 +0100 Subject: [PATCH 19/37] ros: CMakeLists: small fixes and added offboard demo node --- CMakeLists.txt | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c3e8943886..f7c694955d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + geometry_msgs message_generation cmake_modules gazebo_msgs @@ -235,24 +236,33 @@ target_link_libraries(mc_mixer px4 ) -## Commander +## Commander dummy add_executable(commander src/platforms/ros/nodes/commander/commander.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +add_dependencies(commander ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(commander ${catkin_LIBRARIES} px4 ) -## Mavlink +## Mavlink dummy add_executable(mavlink src/platforms/ros/nodes/mavlink/mavlink.cpp) -add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp) +add_dependencies(mavlink ${PROJECT_NAME}_generate_messages_cpp_cpp) target_link_libraries(mavlink ${catkin_LIBRARIES} px4 ) +## Offboard Position Setpoint Demo +add_executable(demo_offboard_position_setpoints + src/platforms/ros/nodes/demo_offboard_position_setpoints/demo_offboard_position_setpoints.cpp) +add_dependencies(demo_offboard_position_setpoints ${PROJECT_NAME}_generate_messages_cpp_cpp) +target_link_libraries(demo_offboard_position_setpoints + ${catkin_LIBRARIES} + px4 +) + ############# ## Install ## ############# From c4b4c5fa41af70b3328486cfa91eb4cf041a1fdb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 15 Feb 2015 20:55:23 +0100 Subject: [PATCH 20/37] fix year in file header --- .../demo_offboard_position_setpoints.cpp | 2 +- .../demo_offboard_position_setpoints.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 index c7dc358dd5..7366d7fc61 100644 --- 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 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 index 14187a9cf8..7d39690f4c 100644 --- 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 @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * 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 From 03e900d3f18c164a7693d3a66e2b69d6353fe733 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 20:14:58 +0100 Subject: [PATCH 21/37] rename ros launch file --- ...ch => gazebo_ardrone_empty_world_offboard_positiondemo.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/{gazebo_ardrone_empty_world_offboard_demo.launch => gazebo_ardrone_empty_world_offboard_positiondemo.launch} (100%) diff --git a/launch/gazebo_ardrone_empty_world_offboard_demo.launch b/launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch similarity index 100% rename from launch/gazebo_ardrone_empty_world_offboard_demo.launch rename to launch/gazebo_ardrone_empty_world_offboard_positiondemo.launch From 80fbb512c98f7a026dacd8da1da0a319496db4ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 22:02:41 +0100 Subject: [PATCH 22/37] ros: mavlink node: update to latest offboard code --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 36 ++++++++++++++------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 013788ecd8..29ffe68ed4 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -124,16 +124,30 @@ void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t c void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) { - mavlink_set_attitude_target_t set_att_target; - mavlink_msg_set_attitude_target_decode(mmsg, &set_att_target); + mavlink_set_attitude_target_t set_attitude_target; + mavlink_msg_set_attitude_target_decode(mmsg, &set_attitude_target); offboard_control_mode offboard_control_mode; - /* set correct ignore flags for body rate fields: copy from mavlink message */ - offboard_control_mode.ignore_bodyrate = (bool)(set_att_target.type_mask & 0x7); + /* set correct ignore flags for thrust field: copy from mavlink message */ - offboard_control_mode.ignore_thrust = (bool)(set_att_target.type_mask & (1 << 6)); - /* set correct ignore flags for attitude field: copy from mavlink message */ - offboard_control_mode.ignore_attitude = (bool)(set_att_target.type_mask & (1 << 7)); + 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.timestamp = get_time_micros(); _offboard_control_mode_pub.publish(offboard_control_mode); @@ -145,13 +159,13 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ att_sp.timestamp = get_time_micros(); - mavlink_quaternion_to_euler(set_att_target.q, &att_sp.roll_body, &att_sp.pitch_body, + 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_att_target.q, (float(*)[3])att_sp.R_body.data()); + mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); att_sp.R_valid = true; - att_sp.thrust = set_att_target.thrust; + att_sp.thrust = set_attitude_target.thrust; for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_att_target.q[i]; + att_sp.q_d[i] = set_attitude_target.q[i]; } att_sp.q_d_valid = true; From 27511324ff41caa069505524f646c25835fb796d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 17 Feb 2015 22:03:20 +0100 Subject: [PATCH 23/37] ros: add offboard attitude sp demo (WIP) --- CMakeLists.txt | 9 ++ ...e_empty_world_offboard_attitudedemo.launch | 10 +++ .../demo_offboard_attitude_setpoints.cpp | 83 +++++++++++++++++++ .../demo_offboard_attitude_setpoints.h | 58 +++++++++++++ 4 files changed, 160 insertions(+) create mode 100644 launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch create mode 100644 src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp create mode 100644 src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f7c694955d..01b52a436d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 ## ############# diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch new file mode 100644 index 0000000000..3998409de8 --- /dev/null +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -0,0 +1,10 @@ + + + + + + + + + + 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 0000000000..402d477832 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -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 +*/ + +#include "demo_offboard_attitude_setpoints.h" + +#include +#include +#include + +DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : + _n(), + _attitude_sp_pub(_n.advertise("mavros/setpoint/attitude", 1)), + _thrust_sp_pub(_n.advertise("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(); +} diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h new file mode 100644 index 0000000000..d7b7a37ba0 --- /dev/null +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.h @@ -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 +*/ + +#include "ros/ros.h" +#include + +class DemoOffboardAttitudeSetpoints +{ +public: + DemoOffboardAttitudeSetpoints(); + + ~DemoOffboardAttitudeSetpoints() {} + + int main(); + +protected: + ros::NodeHandle _n; + ros::Publisher _attitude_sp_pub; + ros::Publisher _thrust_sp_pub; +}; From edbf6204588657b72408efb83c6874b4f7ae6c1e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:09:41 +0100 Subject: [PATCH 24/37] ros: fix offboard attitude demo launch file --- launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch index 3998409de8..2b9d797f6e 100644 --- a/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch +++ b/launch/gazebo_ardrone_empty_world_offboard_attitudedemo.launch @@ -4,7 +4,7 @@ - + From 5b0423109f631763f7e71311d4af7ffa4805edda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:11:36 +0100 Subject: [PATCH 25/37] ros mavlink dummy node: improve offboard attitude setpoint handling --- src/platforms/ros/nodes/mavlink/mavlink.cpp | 24 +++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 29ffe68ed4..5459fcffdc 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -127,7 +127,7 @@ 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); - offboard_control_mode offboard_control_mode; + 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)); @@ -148,11 +148,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) 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); - vehicle_attitude_setpoint att_sp; + 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. @@ -163,14 +166,23 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body.data()); att_sp.R_valid = true; - att_sp.thrust = set_attitude_target.thrust; - for (ssize_t i = 0; i < 4; i++) { - att_sp.q_d[i] = set_attitude_target.q[i]; + + 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; } - 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) From 7ff84c0dcf48e128ae98c52ae6affd43aa46d341 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:12:04 +0100 Subject: [PATCH 26/37] ros: offboard attitude demo node: make quad jump --- .../demo_offboard_attitude_setpoints.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 index 402d477832..027b29a875 100644 --- 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 @@ -44,6 +44,7 @@ #include #include #include +#include DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), @@ -69,7 +70,7 @@ int DemoOffboardAttitudeSetpoints::main() _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.5; + thrust.data = 0.4f + 0.25 * (sinf(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } return 0; From 4bf3107faf50f821cda8715252357754b0ee844a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:54:04 +0100 Subject: [PATCH 27/37] mavros sitl: make it listen to the attitude setpoint topic --- launch/mavros_sitl.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/mavros_sitl.launch b/launch/mavros_sitl.launch index 29f7dd857f..582fdaa7d2 100644 --- a/launch/mavros_sitl.launch +++ b/launch/mavros_sitl.launch @@ -8,6 +8,8 @@ + + From e5d54a487fbca638a52d8c5a12ef4374680cdf96 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 21 Feb 2015 11:54:31 +0100 Subject: [PATCH 28/37] ros offboard attitude sp demo: move attitude --- CMakeLists.txt | 1 + package.xml | 2 ++ .../demo_offboard_attitude_setpoints.cpp | 9 +++++---- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 01b52a436d..f015e56184 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs mav_msgs libmavconn + tf ) find_package(Eigen REQUIRED) diff --git a/package.xml b/package.xml index 96d622a682..28b682c008 100644 --- a/package.xml +++ b/package.xml @@ -45,11 +45,13 @@ std_msgs eigen libmavconn + tf roscpp rospy std_msgs eigen libmavconn + tf 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 index 027b29a875..fb0b09de1e 100644 --- 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 @@ -45,6 +45,7 @@ #include #include #include +#include DemoOffboardAttitudeSetpoints::DemoOffboardAttitudeSetpoints() : _n(), @@ -64,13 +65,13 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - pose.pose.position.x = 0; - pose.pose.position.y = 0; - pose.pose.position.z = 1; + 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(2.0f * (float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + 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; From cbbc660b88917fd0c4cf3fe54ca5c769775e7ae9 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 12:35:45 +0100 Subject: [PATCH 29/37] arm automatically when offboard control mode is set --- src/platforms/ros/nodes/commander/commander.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 9ca54339df..0c32026f39 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -205,8 +205,25 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* 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); } } From a54849eeffde2cfefd5b8274fc2d7ef2da4e92e6 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 14:54:32 +0100 Subject: [PATCH 30/37] adding previous integration demo tests --- CMakeLists.txt | 8 ++ integrationtests/demo_tests/arm_test.py | 37 +++++++ integrationtests/demo_tests/demo_tests.launch | 18 ++++ integrationtests/demo_tests/manual_input.py | 59 ++++++++++++ integrationtests/demo_tests/posctl_test.py | 96 +++++++++++++++++++ integrationtests/integrationtests.launch | 15 +++ package.xml | 1 + 7 files changed, 234 insertions(+) create mode 100755 integrationtests/demo_tests/arm_test.py create mode 100644 integrationtests/demo_tests/demo_tests.launch create mode 100755 integrationtests/demo_tests/manual_input.py create mode 100755 integrationtests/demo_tests/posctl_test.py create mode 100644 integrationtests/integrationtests.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index f015e56184..ca8e114883 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ include_directories( src/ src/lib ${EIGEN_INCLUDE_DIRS} + integrationtests ) ## generate multiplatform wrapper headers @@ -320,3 +321,10 @@ install(TARGETS ${PROJECT_NAME} ## Add folders to be run by python nosetests # catkin_add_nosetests(test) + +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest(integrationtests/integrationtests.launch) +endif() + + diff --git a/integrationtests/demo_tests/arm_test.py b/integrationtests/demo_tests/arm_test.py new file mode 100755 index 0000000000..569e0af7ce --- /dev/null +++ b/integrationtests/demo_tests/arm_test.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import actuator_armed +from manual_input import ManualInput + +class ArmTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + # + # Test arming + # + def test_arm(self): + rospy.init_node('test_node', anonymous=True) + sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + + # method to test + arm = ManualInput() + arm.arm() + + self.assertEquals(self.actuatorStatus.armed, True, "not armed") + + + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch new file mode 100644 index 0000000000..b5377659f9 --- /dev/null +++ b/integrationtests/demo_tests/demo_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py new file mode 100755 index 0000000000..4bf5085091 --- /dev/null +++ b/integrationtests/demo_tests/manual_input.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python +import sys +import rospy + +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + + +# +# Manual input control helper, fakes joystick input +# > needs to correspond to default mapping in manual_input node +# +class ManualInput: + + def __init__(self): + rospy.init_node('test_node', anonymous=True) + self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) + self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + + def arm(self): + rate = rospy.Rate(10) # 10hz + + msg = Joy() + msg.header = Header() + msg.buttons = [0, 0, 0, 0, 0] + msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("zeroing") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 + + msg.buttons = [0, 0, 0, 0, 0] + msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("arming") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 + + def posctl(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + msg = Joy() + msg.header = Header() + msg.buttons = [0, 0, 1, 0, 0] + msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + self.joyPx4.publish(msg) + self.joyIris.publish(msg) + rate.sleep() + count = count + 1 diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/posctl_test.py new file mode 100755 index 0000000000..1cbf09cf73 --- /dev/null +++ b/integrationtests/demo_tests/posctl_test.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +PKG = 'px4' + +import sys +import unittest +import rospy + +from px4.msg import vehicle_local_position +from px4.msg import vehicle_control_mode +from px4.msg import actuator_armed +from px4.msg import position_setpoint_triplet +from px4.msg import position_setpoint +from sensor_msgs.msg import Joy +from std_msgs.msg import Header + +from manual_input import ManualInput + + +class PosctlTest(unittest.TestCase): + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + + # + # Test POSCTL + # + def test_posctl(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + rate = rospy.Rate(10) # 10hz + + manIn = ManualInput() + + # arm and go into POSCTL + manIn.arm() + manIn.posctl() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = 2 + pos.z = -2 + pos.y = 2 + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + timeout = 120 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + positionHeld = False + break + count = count + 1 + rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', PosctlTest) diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch new file mode 100644 index 0000000000..2e59b56155 --- /dev/null +++ b/integrationtests/integrationtests.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 28b682c008..2da49096f9 100644 --- a/package.xml +++ b/package.xml @@ -46,6 +46,7 @@ eigen libmavconn tf + rostest roscpp rospy std_msgs From e0e7f8c5177b813b3e6e8eb0477f9ee32c06c407 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Sun, 22 Feb 2015 18:02:43 +0100 Subject: [PATCH 31/37] - updated manual input to publish directly to px4 and not over joystick topics - updated tests to work with current setup --- integrationtests/demo_tests/demo_tests.launch | 9 +- integrationtests/demo_tests/manual_input.py | 95 ++++++++++++++----- ...posctl_test.py => offboard_posctl_test.py} | 10 +- launch/gazebo_ardrone_empty_world.launch | 2 +- 4 files changed, 84 insertions(+), 32 deletions(-) rename integrationtests/demo_tests/{posctl_test.py => offboard_posctl_test.py} (93%) diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch index b5377659f9..34281b781a 100644 --- a/integrationtests/demo_tests/demo_tests.launch +++ b/integrationtests/demo_tests/demo_tests.launch @@ -1,8 +1,8 @@ - - + + - + @@ -12,7 +12,8 @@ + - + diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 4bf5085091..eb9144bbcb 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -2,43 +2,59 @@ import sys import rospy -from sensor_msgs.msg import Joy +from px4.msg import manual_control_setpoint +from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header - # -# Manual input control helper, fakes joystick input -# > needs to correspond to default mapping in manual_input node +# Manual input control helper +# +# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else +# the simulator does not instantiate our controller. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) - self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("zeroing") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) rate.sleep() count = count + 1 - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos.r = 1 count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("arming") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 @@ -46,14 +62,49 @@ class ManualInput: rate = rospy.Rate(10) # 10hz # triggers posctl - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 1, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("triggering posctl") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 + + def offboard(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py similarity index 93% rename from integrationtests/demo_tests/posctl_test.py rename to integrationtests/demo_tests/offboard_posctl_test.py index 1cbf09cf73..0d56082459 100755 --- a/integrationtests/demo_tests/posctl_test.py +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -16,7 +16,7 @@ from std_msgs.msg import Header from manual_input import ManualInput -class PosctlTest(unittest.TestCase): +class OffboardPosctlTest(unittest.TestCase): # # General callback functions used in tests @@ -37,7 +37,7 @@ class PosctlTest(unittest.TestCase): return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) # - # Test POSCTL + # Test offboard POSCTL # def test_posctl(self): rospy.init_node('test_node', anonymous=True) @@ -48,9 +48,9 @@ class PosctlTest(unittest.TestCase): manIn = ManualInput() - # arm and go into POSCTL + # arm and go into offboard manIn.arm() - manIn.posctl() + manIn.offboard() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") @@ -93,4 +93,4 @@ class PosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', PosctlTest) + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 9d19fe5f9a..22bfb0c799 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ - + From 75f1678047e2beb4ec4e1cf7fd383685175d3694 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 18:18:08 +0100 Subject: [PATCH 32/37] - updated position control test - added flight path assertion helper --- .../demo_tests/flight_path_assertion.py | 122 ++++++++++++++++++ .../demo_tests/offboard_posctl_test.py | 85 +++++++----- 2 files changed, 177 insertions(+), 30 deletions(-) create mode 100644 integrationtests/demo_tests/flight_path_assertion.py diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py new file mode 100644 index 0000000000..fb78f89298 --- /dev/null +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python +import sys +import rospy +import threading + +from px4.msg import vehicle_local_position +from gazebo_msgs.srv import SpawnModel +from gazebo_msgs.srv import SetModelState +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist + +from numpy import linalg +import numpy as np +import math + +# +# Helper to test if vehicle stays in expected flight path. +# +class FlightPathAssertion(threading.Thread): + + def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): + threading.Thread.__init__(self) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.spawn = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.positions = positions + self.tunnelRadius = tunnelRadius + self.yawOffset = yawOffset + self.hasPos = False + self.shouldStop = False + self.center = positions[0] + self.endOfSegment = False + + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def spawn_indicator(self): + xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius + self.spawn("indicator", xml, "", Pose(), "") + + def position_indicator(self): + state = SetModelState() + state.model_name = "indicator" + pose = Pose() + pose.position.x = self.center[0] + pose.position.y = (-1) * self.center[1] + pose.position.z = (-1) * self.center[2] + state.pose = pose + state.twist = Twist() + state.reference_frame = "" + self.setModelState(state) + + def distance_to_line(self, a, b, pos): + v = b - a + w = pos - a + + c1 = np.dot(w, v) + if c1 <= 0: # before a + self.center = a + return linalg.norm(pos - a) + + c2 = np.dot(v, v) + if c2 <= c1: # after b + self.center = b + self.endOfSegment = True + return linalg.norm(pos - b) + + x = c1 / c2 + l = a + x * v + self.center = l + return linalg.norm(pos - l) + + def stop(self): + self.shouldStop = True + + def run(self): + rate = rospy.Rate(10) # 10hz + self.spawn_indicator() + + current = 0 + + while not self.shouldStop: + if (self.hasPos): + # calculate distance to line segment between first two points + # if distances > tunnelRadius + # exit with error + # advance current pos if not on the line anymore or distance to next point < tunnelRadius + # exit if current pos is now the last position + + self.position_indicator() + + pos = np.array((self.localPosition.x, + self.localPosition.y, + self.localPosition.z)) + aPos = np.array((self.positions[current][0], + self.positions[current][1], + self.positions[current][2])) + bPos = np.array((self.positions[current + 1][0], + self.positions[current + 1][1], + self.positions[current + 1][2])) + + dist = self.distance_to_line(aPos, bPos, pos) + bDist = linalg.norm(pos - bPos) + + rospy.loginfo("distance to line: %f, distance to end: %f" % (dist, bDist)) + + if (dist > self.tunnelRadius): + rospy.logerr("left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + # FIXME: assertion + break + + if (self.endOfSegment or bDist < self.tunnelRadius): + rospy.loginfo("next segment") + self.endOfSegment = False + current = current + 1 + + if (current == len(self.positions) - 1): + rospy.loginfo("no more positions") + break + + rate.sleep() diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py index 0d56082459..af8d4b821b 100755 --- a/integrationtests/demo_tests/offboard_posctl_test.py +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -5,6 +5,9 @@ import sys import unittest import rospy +from numpy import linalg +import numpy as np + from px4.msg import vehicle_local_position from px4.msg import vehicle_control_mode from px4.msg import actuator_armed @@ -14,10 +17,18 @@ from sensor_msgs.msg import Joy from std_msgs.msg import Header from manual_input import ManualInput +from flight_path_assertion import FlightPathAssertion class OffboardPosctlTest(unittest.TestCase): + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + # # General callback functions used in tests # @@ -33,19 +44,37 @@ class OffboardPosctlTest(unittest.TestCase): # Helper methods # def is_at_position(self, x, y, z, offset): - rospy.loginfo("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.publish(stp) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") # # Test offboard POSCTL # def test_posctl(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - rate = rospy.Rate(10) # 10hz - manIn = ManualInput() # arm and go into offboard @@ -55,42 +84,38 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = 2 - pos.z = -2 - pos.y = 2 - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - pubSpt.publish(stp) + # prepare flight path assertion + fpa = FlightPathAssertion( + ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2), + ), 0.5, 0) + fpa.start() - # does it reach the position in X seconds? - count = 0 - timeout = 120 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - break - count = count + 1 - rate.sleep() - - self.assertTrue(count < timeout, "took too long to get to position") + self.reach_position(2, 2, -2, 120) + self.reach_position(2, -2, -2, 120) + self.reach_position(-2, -2, -2, 120) + self.reach_position(2, 2, -2, 120) # does it hold the position for Y seconds? positionHeld = True count = 0 timeout = 50 while(count < timeout): - if(not self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + if(not self.is_at_position(2, 2, -2, 0.5)): positionHeld = False break count = count + 1 - rate.sleep() + self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") + fpa.stop() if __name__ == '__main__': import rostest rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() From 85ac3e3515bc214a770074182617208b24ee0209 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 18:33:10 +0100 Subject: [PATCH 33/37] renamed tests, added placeholder for mavros test --- integrationtests/demo_tests/demo_tests.launch | 19 ------------------- .../{arm_test.py => direct_arm_test.py} | 0 ...test.py => direct_offboard_posctl_test.py} | 0 .../demo_tests/direct_tests.launch | 18 ++++++++++++++++++ .../demo_tests/mavros_tests.launch | 18 ++++++++++++++++++ integrationtests/integrationtests.launch | 15 ++------------- 6 files changed, 38 insertions(+), 32 deletions(-) delete mode 100644 integrationtests/demo_tests/demo_tests.launch rename integrationtests/demo_tests/{arm_test.py => direct_arm_test.py} (100%) rename integrationtests/demo_tests/{offboard_posctl_test.py => direct_offboard_posctl_test.py} (100%) create mode 100644 integrationtests/demo_tests/direct_tests.launch create mode 100644 integrationtests/demo_tests/mavros_tests.launch diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch deleted file mode 100644 index 34281b781a..0000000000 --- a/integrationtests/demo_tests/demo_tests.launch +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - diff --git a/integrationtests/demo_tests/arm_test.py b/integrationtests/demo_tests/direct_arm_test.py similarity index 100% rename from integrationtests/demo_tests/arm_test.py rename to integrationtests/demo_tests/direct_arm_test.py diff --git a/integrationtests/demo_tests/offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py similarity index 100% rename from integrationtests/demo_tests/offboard_posctl_test.py rename to integrationtests/demo_tests/direct_offboard_posctl_test.py diff --git a/integrationtests/demo_tests/direct_tests.launch b/integrationtests/demo_tests/direct_tests.launch new file mode 100644 index 0000000000..d871c085cf --- /dev/null +++ b/integrationtests/demo_tests/direct_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch new file mode 100644 index 0000000000..f5ad3412d0 --- /dev/null +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch index 2e59b56155..7f6368d12b 100644 --- a/integrationtests/integrationtests.launch +++ b/integrationtests/integrationtests.launch @@ -1,15 +1,4 @@ - - - - - - - - - - - - - + + From 952f91738ea0704fe5dbbb1598a17ca53f3a70a0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 21:45:01 +0100 Subject: [PATCH 34/37] update flight path assertion and error handling --- .../demo_tests/direct_offboard_posctl_test.py | 35 ++++++++++--------- .../demo_tests/flight_path_assertion.py | 23 +++++++++--- 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index af8d4b821b..6b28e0a18d 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -29,6 +29,10 @@ class OffboardPosctlTest(unittest.TestCase): self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) self.rate = rospy.Rate(10) # 10hz + def tearDown(self): + if (self.fpa): + self.fpa.stop() + # # General callback functions used in tests # @@ -83,22 +87,21 @@ class OffboardPosctlTest(unittest.TestCase): self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - - # prepare flight path assertion - fpa = FlightPathAssertion( - ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2), - ), 0.5, 0) - fpa.start() - self.reach_position(2, 2, -2, 120) - self.reach_position(2, -2, -2, 120) - self.reach_position(-2, -2, -2, 120) - self.reach_position(2, 2, -2, 120) + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) + + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) # does it hold the position for Y seconds? positionHeld = True @@ -112,7 +115,7 @@ class OffboardPosctlTest(unittest.TestCase): self.rate.sleep() self.assertTrue(count == timeout, "position could not be held") - fpa.stop() + self.fpa.stop() if __name__ == '__main__': diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index fb78f89298..1d99b7e5ac 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -6,6 +6,7 @@ import threading from px4.msg import vehicle_local_position from gazebo_msgs.srv import SpawnModel from gazebo_msgs.srv import SetModelState +from gazebo_msgs.srv import DeleteModel from geometry_msgs.msg import Pose from geometry_msgs.msg import Twist @@ -18,11 +19,20 @@ import math # class FlightPathAssertion(threading.Thread): + # + # Arguments + # - positions: tuple of tuples in the form (x, y, z, heading) + # + # TODO: yaw validation + # TODO: fail main test thread + # def __init__(self, positions, tunnelRadius = 1, yawOffset = 0.2): threading.Thread.__init__(self) rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawn = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) + self.spawnModel = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) self.setModelState = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) + self.deleteModel = rospy.ServiceProxy('gazebo/delete_model', DeleteModel) + self.positions = positions self.tunnelRadius = tunnelRadius self.yawOffset = yawOffset @@ -30,14 +40,16 @@ class FlightPathAssertion(threading.Thread): self.shouldStop = False self.center = positions[0] self.endOfSegment = False + self.failed = False def position_callback(self, data): self.hasPos = True self.localPosition = data def spawn_indicator(self): + self.deleteModel("indicator") xml = "true0.7%f1 0 0 0.51 0 0 0.5" % self.tunnelRadius - self.spawn("indicator", xml, "", Pose(), "") + self.spawnModel("indicator", xml, "", Pose(), "") def position_indicator(self): state = SetModelState() @@ -103,11 +115,12 @@ class FlightPathAssertion(threading.Thread): dist = self.distance_to_line(aPos, bPos, pos) bDist = linalg.norm(pos - bPos) - rospy.loginfo("distance to line: %f, distance to end: %f" % (dist, bDist)) + rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, bDist)) if (dist > self.tunnelRadius): - rospy.logerr("left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - # FIXME: assertion + msg = "left tunnel at position (%f, %f, %f)" % (self.localPosition.x, self.localPosition.y, self.localPosition.z) + rospy.logerr(msg) + self.failed = True break if (self.endOfSegment or bDist < self.tunnelRadius): From 638022f7ad6cd3bff61e264e8a80940ff23d084c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 22:02:26 +0100 Subject: [PATCH 35/37] fix headers and indentation --- .../demo_tests/direct_arm_test.py | 74 +++++-- .../demo_tests/direct_offboard_posctl_test.py | 204 ++++++++++-------- .../demo_tests/flight_path_assertion.py | 36 ++++ integrationtests/demo_tests/manual_input.py | 200 ++++++++++------- 4 files changed, 329 insertions(+), 185 deletions(-) diff --git a/integrationtests/demo_tests/direct_arm_test.py b/integrationtests/demo_tests/direct_arm_test.py index 569e0af7ce..238f2d7e0f 100755 --- a/integrationtests/demo_tests/direct_arm_test.py +++ b/integrationtests/demo_tests/direct_arm_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -10,28 +46,28 @@ from manual_input import ManualInput class ArmTest(unittest.TestCase): - # - # General callback functions used in tests - # - def actuator_armed_callback(self, data): - self.actuatorStatus = data - - # - # Test arming - # - def test_arm(self): - rospy.init_node('test_node', anonymous=True) - sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) + # + # General callback functions used in tests + # + def actuator_armed_callback(self, data): + self.actuatorStatus = data + + # + # Test arming + # + def test_arm(self): + rospy.init_node('test_node', anonymous=True) + sub = rospy.Subscriber('px4_multicopter/actuator_armed', actuator_armed, self.actuator_armed_callback) - # method to test - arm = ManualInput() - arm.arm() + # method to test + arm = ManualInput() + arm.arm() - self.assertEquals(self.actuatorStatus.armed, True, "not armed") + self.assertEquals(self.actuatorStatus.armed, True, "not armed") - + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'arm_test', ArmTest) + import rostest + rostest.rosrun(PKG, 'arm_test', ArmTest) diff --git a/integrationtests/demo_tests/direct_offboard_posctl_test.py b/integrationtests/demo_tests/direct_offboard_posctl_test.py index 6b28e0a18d..42667757b0 100755 --- a/integrationtests/demo_tests/direct_offboard_posctl_test.py +++ b/integrationtests/demo_tests/direct_offboard_posctl_test.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# PKG = 'px4' import sys @@ -22,103 +58,103 @@ from flight_path_assertion import FlightPathAssertion class OffboardPosctlTest(unittest.TestCase): - def setUp(self): - rospy.init_node('test_node', anonymous=True) - rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) - rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) - self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) - self.rate = rospy.Rate(10) # 10hz + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/vehicle_local_position", vehicle_local_position, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/position_setpoint_triplet', position_setpoint_triplet, queue_size=10) + self.rate = rospy.Rate(10) # 10hz - def tearDown(self): - if (self.fpa): - self.fpa.stop() + def tearDown(self): + if (self.fpa): + self.fpa.stop() - # - # General callback functions used in tests - # - def position_callback(self, data): - self.hasPos = True - self.localPosition = data + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data - def vehicle_control_mode_callback(self, data): - self.controlMode = data + def vehicle_control_mode_callback(self, data): + self.controlMode = data - # - # Helper methods - # - def is_at_position(self, x, y, z, offset): - rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) - desired = np.array((x, y, z)) - pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) - return linalg.norm(desired - pos) < offset + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.x, self.localPosition.y, self.localPosition.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.x, self.localPosition.y, self.localPosition.z)) + return linalg.norm(desired - pos) < offset - def reach_position(self, x, y, z, timeout): - # set a position setpoint - pos = position_setpoint() - pos.valid = True - pos.x = x - pos.y = y - pos.z = z - pos.position_valid = True - stp = position_setpoint_triplet() - stp.current = pos - self.pubSpt.publish(stp) + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = position_setpoint() + pos.valid = True + pos.x = x + pos.y = y + pos.z = z + pos.position_valid = True + stp = position_setpoint_triplet() + stp.current = pos + self.pubSpt.publish(stp) - # does it reach the position in X seconds? - count = 0 - while(count < timeout): - if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): - break - count = count + 1 - self.rate.sleep() + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + if(self.is_at_position(pos.x, pos.y, pos.z, 0.5)): + break + count = count + 1 + self.rate.sleep() - self.assertTrue(count < timeout, "took too long to get to position") + self.assertTrue(count < timeout, "took too long to get to position") - # - # Test offboard POSCTL - # - def test_posctl(self): - manIn = ManualInput() + # + # Test offboard POSCTL + # + def test_posctl(self): + manIn = ManualInput() - # arm and go into offboard - manIn.arm() - manIn.offboard() - self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") - self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") - self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") + # arm and go into offboard + manIn.arm() + manIn.offboard() + self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") + self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") + self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") - # prepare flight path assertion - positions = ( - (0,0,0), - (2,2,-2), - (2,-2,-2), - (-2,-2,-2), - (2,2,-2)) + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,-2), + (2,-2,-2), + (-2,-2,-2), + (2,2,-2)) - self.fpa = FlightPathAssertion(positions, 1, 0) - self.fpa.start() + self.fpa = FlightPathAssertion(positions, 1, 0) + self.fpa.start() - for i in range(0, len(positions)): - self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) - self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) - - # does it hold the position for Y seconds? - positionHeld = True - count = 0 - timeout = 50 - while(count < timeout): - if(not self.is_at_position(2, 2, -2, 0.5)): - positionHeld = False - break - count = count + 1 - self.rate.sleep() + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, -2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() - self.assertTrue(count == timeout, "position could not be held") - self.fpa.stop() - + self.assertTrue(count == timeout, "position could not be held") + self.fpa.stop() + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) - #unittest.main() + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/flight_path_assertion.py b/integrationtests/demo_tests/flight_path_assertion.py index 1d99b7e5ac..1f5bf01fc8 100644 --- a/integrationtests/demo_tests/flight_path_assertion.py +++ b/integrationtests/demo_tests/flight_path_assertion.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy import threading diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index eb9144bbcb..55911bede6 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -1,4 +1,40 @@ #!/usr/bin/env python +#*************************************************************************** +# +# 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# import sys import rospy @@ -14,97 +50,97 @@ from std_msgs.msg import Header # class ManualInput: - def __init__(self): - rospy.init_node('test_node', anonymous=True) - self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) - self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) + def __init__(self): + rospy.init_node('test_node', anonymous=True) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) - def arm(self): - rate = rospy.Rate(10) # 10hz + def arm(self): + rate = rospy.Rate(10) # 10hz - att = CommandAttitudeThrust() - att.header = Header() + att = CommandAttitudeThrust() + att.header = Header() - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("zeroing") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - # Fake input to iris commander - self.pubAtt.publish(att) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("zeroing") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) + rate.sleep() + count = count + 1 - pos.r = 1 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("arming") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + pos.r = 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("arming") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 - def posctl(self): - rate = rospy.Rate(10) # 10hz + def posctl(self): + rate = rospy.Rate(10) # 10hz - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 2 - pos.return_switch = 3 - pos.posctl_switch = 1 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 3 + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 - def offboard(self): - rate = rospy.Rate(10) # 10hz + def offboard(self): + rate = rospy.Rate(10) # 10hz - # triggers posctl - pos = manual_control_setpoint() - pos.x = 0 - pos.z = 0 - pos.y = 0 - pos.r = 0 - pos.mode_switch = 3 - pos.return_switch = 3 - pos.posctl_switch = 3 - pos.loiter_switch = 3 - pos.acro_switch = 0 - pos.offboard_switch = 1 + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 - count = 0 - while not rospy.is_shutdown() and count < 10: - rospy.loginfo("triggering posctl") - time = rospy.get_rostime().now() - pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 - self.pubMcsp.publish(pos) - rate.sleep() - count = count + 1 + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 From b955b9391d611706d1fd022c527426cab6a54924 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 23:35:06 +0100 Subject: [PATCH 36/37] added mavros offboard test --- .../demo_tests/mavros_offboard_posctl_test.py | 146 ++++++++++++++++++ .../demo_tests/mavros_tests.launch | 4 +- 2 files changed, 148 insertions(+), 2 deletions(-) create mode 100755 integrationtests/demo_tests/mavros_offboard_posctl_test.py diff --git a/integrationtests/demo_tests/mavros_offboard_posctl_test.py b/integrationtests/demo_tests/mavros_offboard_posctl_test.py new file mode 100755 index 0000000000..7468ad53f0 --- /dev/null +++ b/integrationtests/demo_tests/mavros_offboard_posctl_test.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python +#*************************************************************************** +# +# 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. +# +#***************************************************************************/ + +# +# @author Andreas Antener +# +PKG = 'px4' + +import sys +import unittest +import rospy +import math + +from numpy import linalg +import numpy as np + +from px4.msg import vehicle_control_mode +from std_msgs.msg import Header +from geometry_msgs.msg import PoseStamped, Quaternion +from tf.transformations import quaternion_from_euler + +class OffboardPosctlTest(unittest.TestCase): + + def setUp(self): + rospy.init_node('test_node', anonymous=True) + rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) + rospy.Subscriber("px4_multicopter/mavros/position/local", PoseStamped, self.position_callback) + self.pubSpt = rospy.Publisher('px4_multicopter/mavros/setpoint/local_position', PoseStamped, queue_size=10) + self.rate = rospy.Rate(10) # 10hz + self.hasPos = False + + # + # General callback functions used in tests + # + def position_callback(self, data): + self.hasPos = True + self.localPosition = data + + def vehicle_control_mode_callback(self, data): + self.controlMode = data + + + # + # Helper methods + # + def is_at_position(self, x, y, z, offset): + if(not self.hasPos): + return False + + rospy.logdebug("current position %f, %f, %f" % (self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + desired = np.array((x, y, z)) + pos = np.array((self.localPosition.pose.position.x, self.localPosition.pose.position.y, self.localPosition.pose.position.z)) + return linalg.norm(desired - pos) < offset + + def reach_position(self, x, y, z, timeout): + # set a position setpoint + pos = PoseStamped() + pos.header = Header() + pos.header.frame_id = "base_footprint" + pos.header.stamp = rospy.Time.now() + pos.pose.position.x = x + pos.pose.position.y = y + pos.pose.position.z = z + + # For demo purposes we will lock yaw/heading to north. + yaw_degrees = 0 # North + yaw = math.radians(yaw_degrees) + quaternion = quaternion_from_euler(0, 0, yaw) + pos.pose.orientation = Quaternion(*quaternion) + + # does it reach the position in X seconds? + count = 0 + while(count < timeout): + self.pubSpt.publish(pos) + + if(self.is_at_position(pos.pose.position.x, pos.pose.position.y, pos.pose.position.z, 0.5)): + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count < timeout, "took too long to get to position") + + # + # Test offboard POSCTL + # + def test_posctl(self): + # prepare flight path assertion + positions = ( + (0,0,0), + (2,2,2), + (2,-2,2), + (-2,-2,2), + (2,2,2)) + + for i in range(0, len(positions)): + self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) + + # does it hold the position for Y seconds? + positionHeld = True + count = 0 + timeout = 50 + while(count < timeout): + if(not self.is_at_position(2, 2, 2, 0.5)): + positionHeld = False + break + count = count + 1 + self.rate.sleep() + + self.assertTrue(count == timeout, "position could not be held") + + +if __name__ == '__main__': + import rostest + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) + #unittest.main() diff --git a/integrationtests/demo_tests/mavros_tests.launch b/integrationtests/demo_tests/mavros_tests.launch index f5ad3412d0..8c1ad7b4db 100644 --- a/integrationtests/demo_tests/mavros_tests.launch +++ b/integrationtests/demo_tests/mavros_tests.launch @@ -1,5 +1,5 @@ - + @@ -14,5 +14,5 @@ - + From 482f2c94424f32e45aaee68b8b515e1eab40b6de Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 23 Feb 2015 23:45:54 +0100 Subject: [PATCH 37/37] added integration tests to cmake list --- CMakeLists.txt | 3 ++- integrationtests/integrationtests.launch | 4 ---- 2 files changed, 2 insertions(+), 5 deletions(-) delete mode 100644 integrationtests/integrationtests.launch diff --git a/CMakeLists.txt b/CMakeLists.txt index ca8e114883..457a0bfa74 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -324,7 +324,8 @@ install(TARGETS ${PROJECT_NAME} if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(integrationtests/integrationtests.launch) + add_rostest(integrationtests/demo_tests/direct_tests.launch) + add_rostest(integrationtests/demo_tests/mavros_tests.launch) endif() diff --git a/integrationtests/integrationtests.launch b/integrationtests/integrationtests.launch deleted file mode 100644 index 7f6368d12b..0000000000 --- a/integrationtests/integrationtests.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - -