├── common ├── bin │ └── popf ├── problem.pddl ├── rosplan_quad_p1.pddl ├── plan.pddl ├── rosplan_quad.pddl ├── domain.pddl ├── scripts │ └── init_demo.sh └── domainwp.pddl ├── CMakeLists.txt ├── launch ├── indoor_slam_gazebo.launch ├── test_quadrotor_demo.launch ├── test_quadrotor_plan.launch └── quadrotor.launch ├── README.md ├── package.xml └── src ├── quadrotor_teleop.cpp └── rosplan_interface_quadrotor.cpp /common/bin/popf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/fairf4x/rosplan_interface_quadrotor/HEAD/common/bin/popf -------------------------------------------------------------------------------- /common/problem.pddl: -------------------------------------------------------------------------------- 1 | (define (problem rosplan_quad_task) 2 | (:domain rosplan_quad) 3 | (:objects 4 | q1 - quad 5 | ) 6 | (:init 7 | (grounded q1) 8 | ) 9 | (:goal (and 10 | (finished q1) 11 | (squaredone q1) 12 | ))) 13 | -------------------------------------------------------------------------------- /common/rosplan_quad_p1.pddl: -------------------------------------------------------------------------------- 1 | (define (problem p1) 2 | (:domain rosplan_quad) 3 | (:objects q1 - quad) 4 | (:init 5 | (grounded q1) 6 | ) 7 | 8 | (:goal 9 | (and 10 | (finished q1) 11 | (squaredone q1) 12 | ) 13 | ) 14 | ) 15 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rosplan_interface_quadrotor) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | std_srvs 12 | genmsg 13 | mongodb_store 14 | actionlib 15 | geometry_msgs 16 | rosplan_dispatch_msgs 17 | hector_move_base 18 | ) 19 | 20 | catkin_package() 21 | include_directories(include ${catkin_INCLUDE_DIRS}) 22 | 23 | add_executable(rosplan_interface_quadrotor src/rosplan_interface_quadrotor.cpp) 24 | target_link_libraries(rosplan_interface_quadrotor ${catkin_LIBRARIES}) 25 | -------------------------------------------------------------------------------- /common/plan.pddl: -------------------------------------------------------------------------------- 1 | Number of literals: 4 2 | Constructing lookup tables: 3 | Post filtering unreachable actions: 4 | No analytic limits found, not considering limit effects of goal-only operators 5 | Initial heuristic = 3.000 6 | b (2.000 | 0.000)b (1.000 | 0.001) 7 | ; Plan found with metric 0.002 8 | ; States evaluated so far: 5 9 | ; Time 0.00 10 | 0.000: (takeoff q1) [0.001] 11 | 0.001: (flysquare q1) [0.001] 12 | 0.002: (land q1) [0.001] 13 | 14 | * All goal deadlines now no later than 0.002 15 | 16 | Resorting to best-first search 17 | b (2.000 | 0.000)b (1.000 | 0.001) 18 | Problem Unsolvable 19 | ;;;; Solution Found 20 | ; States evaluated: 10 21 | ; Cost: 0.002 22 | ; Time 0.00 23 | 0.000: (takeoff q1) [0.001] 24 | 0.001: (flysquare q1) [0.001] 25 | 0.002: (land q1) [0.001] 26 | -------------------------------------------------------------------------------- /common/rosplan_quad.pddl: -------------------------------------------------------------------------------- 1 | (define (domain rosplan_quad) 2 | 3 | (:requirements 4 | :strips 5 | :typing 6 | ) 7 | 8 | (:predicates 9 | (ready) 10 | (grounded) 11 | (airborne) 12 | (finished) 13 | (squaredone) 14 | ) 15 | 16 | (:action takeoff 17 | :parameters () 18 | :precondition (and (grounded)) 19 | :effect (and (not (grounded)) (ready) (airborne)) 20 | ) 21 | 22 | (:action land 23 | :parameters () 24 | :precondition (and (airborne) (ready)) 25 | :effect (and (not (airborne)) 26 | (grounded) 27 | (not (ready)) 28 | (finished) 29 | ) 30 | ) 31 | 32 | (:action flysquare 33 | :parameters () 34 | :precondition (and (airborne)) 35 | :effect (and (squaredone)) 36 | ) 37 | ) 38 | -------------------------------------------------------------------------------- /common/domain.pddl: -------------------------------------------------------------------------------- 1 | (define (domain rosplan_quad) 2 | 3 | (:requirements 4 | :strips 5 | :typing 6 | ) 7 | 8 | (:predicates 9 | (grounded ?q - quad) 10 | (airborne ?q - quad) 11 | (finished ?q -quad) 12 | (squaredone ?q - quad) 13 | ) 14 | 15 | (:types 16 | quad - object 17 | ) 18 | 19 | (:action takeoff 20 | :parameters (?q - quad) 21 | :precondition (and (grounded ?q)) 22 | :effect (and (not (grounded ?q)) (airborne ?q)) 23 | ) 24 | 25 | (:action land 26 | :parameters (?q - quad) 27 | :precondition (and (airborne ?q)) 28 | :effect (and (not (airborne ?q)) 29 | (finished ?q) 30 | (grounded ?q) 31 | ) 32 | ) 33 | 34 | (:action flysquare 35 | :parameters (?q - quad) 36 | :precondition (and (airborne ?q)) 37 | :effect (and (squaredone ?q)) 38 | ) 39 | ) 40 | -------------------------------------------------------------------------------- /launch/indoor_slam_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /launch/test_quadrotor_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /common/scripts/init_demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # add quadrotor instance 4 | rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 5 | knowledge: 6 | knowledge_type: 0 7 | instance_type: 'quad' 8 | instance_name: 'q1' 9 | attribute_name: '' 10 | function_value: 0.0"; 11 | 12 | # add fact - quadrotor is on the ground 13 | rosservice call /kcl_rosplan/update_knowledge_base "update_type: 0 14 | knowledge: 15 | knowledge_type: 1 16 | instance_type: '' 17 | instance_name: '' 18 | attribute_name: 'grounded' 19 | values: 20 | - {key: 'q', value: 'q1'} 21 | function_value: 0.0"; 22 | 23 | # add goals - end on the ground and fly square 24 | rosservice call /kcl_rosplan/update_knowledge_base "update_type: 1 25 | knowledge: 26 | knowledge_type: 1 27 | instance_type: '' 28 | instance_name: '' 29 | attribute_name: 'finished' 30 | values: 31 | - {key: 'q', value: 'q1'} 32 | function_value: 0.0"; 33 | 34 | rosservice call /kcl_rosplan/update_knowledge_base "update_type: 1 35 | knowledge: 36 | knowledge_type: 1 37 | instance_type: '' 38 | instance_name: '' 39 | attribute_name: 'squaredone' 40 | values: 41 | - {key: 'q', value: 'q1'} 42 | function_value: 0.0"; 43 | 44 | # call planner 45 | rosservice call /kcl_rosplan/planning_server; 46 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ROSPlan_interface_quadrotor 2 | =========================== 3 | 4 | This package implements a simple interface for [hector_quadrotor](http://wiki.ros.org/hector_quadrotor) to [ROSPlan](https://github.com/KCL-Planning/ROSPlan/wiki). 5 | 6 | It features simple planning domain with three operators `takeoff`, `land` and `flysquare` to demonstrate ROSPlan connection to quadrotor drone. 7 | 8 | ### Installation 9 | 10 | Get the prerequisites: 11 | 12 | (for Indigo) 13 | Install ROSPlan according to [instructions](https://github.com/KCL-Planning/ROSPlan#installation). 14 | 15 | Install hector quadrotor demo package: 16 | 17 | ```sh 18 | sudo apt-get install ros-indigo-hector-quadrotor-demo 19 | ``` 20 | 21 | Get the code: 22 | ```sh 23 | cd src/ 24 | git clone https://github.com/fairf4x/ROSPlan_interface_quadrotor.git 25 | ``` 26 | Compile everything: 27 | ```sh 28 | source /opt/ros/indigo/setup.bash 29 | catkin_make 30 | ``` 31 | 32 | ### Running the demo 33 | 34 | Start rviz and gazebo: 35 | 36 | ``` 37 | roslaunch rosplan_interface_quadrotor test_quadrotor_plan.launch 38 | ``` 39 | 40 | Initialize KnowledgeBase: 41 | 42 | ``` 43 | roscd rosplan_interface_quadrotor/common/scripts/ 44 | sh init_demo.sh 45 | ``` 46 | 47 | Expected output: 48 | 49 | The quadrotor will takeoff, fly square and land. 50 | 51 | 52 | -------------------------------------------------------------------------------- /common/domainwp.pddl: -------------------------------------------------------------------------------- 1 | (define (domain rosplan_quad) 2 | 3 | (:requirements :strips :typing :fluents) 4 | 5 | (:types 6 | quad - object 7 | waypoint - object 8 | ) 9 | 10 | (:predicates 11 | (grounded ?q - quad) 12 | (airborne ?q - quad) 13 | (squaredone ?q - quad) 14 | (at_waypoint ?q - quad ?w - waypoint) 15 | (visited ?q - quad ?w - waypoint) 16 | (connected ?from ?to - waypoint) 17 | ) 18 | 19 | (:functions 20 | (distance ?wp1 ?wp2 - waypoint) 21 | ) 22 | 23 | (:action takeoff 24 | :parameters (?q - quad) 25 | :precondition (and (grounded ?q)) 26 | :effect (and (not (grounded ?q)) (airborne ?q)) 27 | ) 28 | 29 | (:action land 30 | :parameters (?q - quad) 31 | :precondition (and (airborne ?q)) 32 | :effect (and (not (airborne ?q)) 33 | (grounded ?q) 34 | ) 35 | ) 36 | 37 | (:action fly_to_waypoint 38 | :parameters (?q - quad ?from ?to - waypoint) 39 | :precondition (and (airborne ?q) 40 | (at_waypoint ?q ?from) 41 | ) 42 | :effect (and (not (at_waypoint ?q ?from)) 43 | (at_waypoint ?q ?to) 44 | (visited ?q ?to) 45 | ) 46 | ) 47 | 48 | (:action flysquare 49 | :parameters (?q - quad) 50 | :precondition (and (airborne ?q)) 51 | :effect (and (squaredone ?q)) 52 | ) 53 | ) 54 | -------------------------------------------------------------------------------- /launch/test_quadrotor_plan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 18 | 19 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosplan_interface_quadrotor 4 | 0.0.0 5 | The rosplan_interface_quadrotor package 6 | fairf4x 7 | 8 | 9 | 10 | 11 | 12 | TODO 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | catkin 39 | 40 | roscpp 41 | sensor_msgs 42 | std_srvs 43 | geometry_msgs 44 | mongodb_store 45 | rosplan_dispatch_msgs 46 | hector_move_base 47 | 48 | 49 | roscpp 50 | sensor_msgs 51 | std_srvs 52 | geometry_msgs 53 | mongodb_store 54 | rosplan_dispatch_msgs 55 | hector_move_base 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /launch/quadrotor.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /src/quadrotor_teleop.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace hector_quadrotor 38 | { 39 | 40 | class Teleop 41 | { 42 | private: 43 | ros::NodeHandle node_handle_; 44 | ros::Subscriber joy_subscriber_; 45 | 46 | ros::Publisher velocity_publisher_, attitude_publisher_, yawrate_publisher_, thrust_publisher_; 47 | geometry_msgs::Twist velocity_; 48 | hector_uav_msgs::AttitudeCommand attitude_; 49 | hector_uav_msgs::ThrustCommand thrust_; 50 | hector_uav_msgs::YawrateCommand yawrate_; 51 | 52 | struct Axis 53 | { 54 | int axis; 55 | double max; 56 | }; 57 | 58 | struct Button 59 | { 60 | int button; 61 | }; 62 | 63 | struct 64 | { 65 | Axis x; 66 | Axis y; 67 | Axis z; 68 | Axis yaw; 69 | } axes_; 70 | 71 | struct 72 | { 73 | Button slow; 74 | } buttons_; 75 | 76 | double slow_factor_; 77 | 78 | public: 79 | Teleop() 80 | { 81 | ros::NodeHandle params("~"); 82 | 83 | params.param("x_axis", axes_.x.axis, 4); 84 | params.param("y_axis", axes_.y.axis, 3); 85 | params.param("z_axis", axes_.z.axis, 2); 86 | params.param("yaw_axis", axes_.yaw.axis, 1); 87 | 88 | params.param("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0); 89 | params.param("slow_button", buttons_.slow.button, 1); 90 | params.param("slow_factor", slow_factor_, 0.2); 91 | 92 | std::string control_mode_str; 93 | params.param("control_mode", control_mode_str, "twist"); 94 | 95 | if (control_mode_str == "twist") 96 | { 97 | params.param("x_velocity_max", axes_.x.max, 2.0); 98 | params.param("y_velocity_max", axes_.y.max, 2.0); 99 | params.param("z_velocity_max", axes_.z.max, 2.0); 100 | 101 | joy_subscriber_ = node_handle_.subscribe("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1)); 102 | velocity_publisher_ = node_handle_.advertise("cmd_vel", 10); 103 | } 104 | else if (control_mode_str == "attitude") 105 | { 106 | params.param("x_roll_max", axes_.x.max, 0.35); 107 | params.param("y_pitch_max", axes_.y.max, 0.35); 108 | params.param("z_thrust_max", axes_.z.max, 25.0); 109 | joy_subscriber_ = node_handle_.subscribe("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1)); 110 | attitude_publisher_ = node_handle_.advertise("command/attitude", 10); 111 | yawrate_publisher_ = node_handle_.advertise("command/yawrate", 10); 112 | thrust_publisher_ = node_handle_.advertise("command/thrust", 10); 113 | } 114 | 115 | } 116 | 117 | ~Teleop() 118 | { 119 | stop(); 120 | } 121 | 122 | void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy) 123 | { 124 | velocity_.linear.x = getAxis(joy, axes_.x); 125 | velocity_.linear.y = getAxis(joy, axes_.y); 126 | velocity_.linear.z = getAxis(joy, axes_.z); 127 | velocity_.angular.z = getAxis(joy, axes_.yaw); 128 | if (getButton(joy, buttons_.slow.button)) 129 | { 130 | velocity_.linear.x *= slow_factor_; 131 | velocity_.linear.y *= slow_factor_; 132 | velocity_.linear.z *= slow_factor_; 133 | velocity_.angular.z *= slow_factor_; 134 | } 135 | velocity_publisher_.publish(velocity_); 136 | } 137 | 138 | void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy) 139 | { 140 | attitude_.roll = getAxis(joy, axes_.x); 141 | attitude_.pitch = getAxis(joy, axes_.y); 142 | attitude_publisher_.publish(attitude_); 143 | 144 | thrust_.thrust = getAxis(joy, axes_.z); 145 | thrust_publisher_.publish(thrust_); 146 | 147 | yawrate_.turnrate = getAxis(joy, axes_.yaw); 148 | if (getButton(joy, buttons_.slow.button)) 149 | { 150 | yawrate_.turnrate *= slow_factor_; 151 | } 152 | yawrate_publisher_.publish(yawrate_); 153 | } 154 | 155 | sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis) 156 | { 157 | if (axis.axis == 0) 158 | {return 0;} 159 | sensor_msgs::Joy::_axes_type::value_type sign = 1.0; 160 | if (axis.axis < 0) 161 | { 162 | sign = -1.0; 163 | axis.axis = -axis.axis; 164 | } 165 | if ((size_t) axis.axis > joy->axes.size()) 166 | {return 0;} 167 | return sign * joy->axes[axis.axis - 1] * axis.max; 168 | } 169 | 170 | sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button) 171 | { 172 | if (button <= 0) 173 | {return 0;} 174 | if ((size_t) button > joy->axes.size()) 175 | {return 0;} 176 | return joy->buttons[button - 1]; 177 | } 178 | 179 | void stop() 180 | { 181 | if(velocity_publisher_.getNumSubscribers() > 0) 182 | { 183 | velocity_ = geometry_msgs::Twist(); 184 | velocity_publisher_.publish(velocity_); 185 | } 186 | if(attitude_publisher_.getNumSubscribers() > 0) 187 | { 188 | attitude_ = hector_uav_msgs::AttitudeCommand(); 189 | attitude_publisher_.publish(attitude_); 190 | } 191 | if(thrust_publisher_.getNumSubscribers() > 0) 192 | { 193 | thrust_ = hector_uav_msgs::ThrustCommand(); 194 | thrust_publisher_.publish(thrust_); 195 | } 196 | if(yawrate_publisher_.getNumSubscribers() > 0) 197 | { 198 | yawrate_ = hector_uav_msgs::YawrateCommand(); 199 | yawrate_publisher_.publish(yawrate_); 200 | } 201 | 202 | } 203 | }; 204 | 205 | } // namespace hector_quadrotor 206 | 207 | int main(int argc, char **argv) 208 | { 209 | ros::init(argc, argv, "quadrotor_teleop"); 210 | 211 | hector_quadrotor::Teleop teleop; 212 | ros::spin(); 213 | 214 | return 0; 215 | } 216 | -------------------------------------------------------------------------------- /src/rosplan_interface_quadrotor.cpp: -------------------------------------------------------------------------------- 1 | //================================================================================================= 2 | // Copyright (c) 2012, Johannes Meyer, TU Darmstadt 3 | // All rights reserved. 4 | 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright 8 | // notice, this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of the Flight Systems and Automatic Control group, 13 | // TU Darmstadt, nor the names of its contributors may be used to 14 | // endorse or promote products derived from this software without 15 | // specific prior written permission. 16 | 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | //================================================================================================= 28 | 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include "rosplan_dispatch_msgs/ActionDispatch.h" 39 | #include "rosplan_dispatch_msgs/ActionFeedback.h" 40 | #include "rosplan_knowledge_msgs/KnowledgeUpdateService.h" 41 | #include "mongodb_store/message_store.h" 42 | 43 | namespace rosplan_interface_quadrotor 44 | { 45 | 46 | class Quadrotor 47 | { 48 | private: 49 | ros::NodeHandle node_handle_; 50 | 51 | // knowledge interface 52 | ros::ServiceClient update_knowledge_client; 53 | 54 | // motor interface 55 | ros::ServiceClient motor_service_client; 56 | 57 | // topic for publishing action feedback 58 | ros::Publisher feedback_publisher_; 59 | 60 | // subscription to receive drone height messages 61 | ros::Subscriber height_subscriber_; 62 | 63 | // topic for publishing geometry_msgs::Twist data 64 | ros::Publisher velocity_publisher_; 65 | 66 | // published velocity 67 | geometry_msgs::Twist velocity_; 68 | 69 | float height_; 70 | 71 | // database - waypoints are retrieved from here 72 | mongodb_store::MessageStoreProxy message_store; 73 | 74 | // method to add/remove simple predicate in the knowledge database 75 | void oneVariablePredicate(std::string pred_name, std::string var_name, std::string var_value, const char& update) 76 | { 77 | rosplan_knowledge_msgs::KnowledgeUpdateService updatePredSrv; 78 | 79 | updatePredSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; 80 | updatePredSrv.request.update_type = update; 81 | updatePredSrv.request.knowledge.attribute_name = pred_name; 82 | 83 | diagnostic_msgs::KeyValue pair; 84 | pair.key = var_name; 85 | pair.value = var_value; 86 | updatePredSrv.request.knowledge.values.push_back(pair); 87 | 88 | update_knowledge_client.call(updatePredSrv); 89 | } 90 | 91 | // method to add/remove simple predicate in the knowledge database 92 | void twoVariablePredicate(std::string pred_name, std::string var1_name, std::string var1_value, std::string var2_name, std::string var2_value, const char& update) 93 | { 94 | rosplan_knowledge_msgs::KnowledgeUpdateService updatePredSrv; 95 | 96 | updatePredSrv.request.knowledge.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; 97 | updatePredSrv.request.update_type = update; 98 | updatePredSrv.request.knowledge.attribute_name = pred_name; 99 | 100 | diagnostic_msgs::KeyValue pair1; 101 | pair1.key = var1_name; 102 | pair1.value = var1_value; 103 | updatePredSrv.request.knowledge.values.push_back(pair1); 104 | 105 | diagnostic_msgs::KeyValue pair2; 106 | pair2.key = var2_name; 107 | pair2.value = var2_value; 108 | updatePredSrv.request.knowledge.values.push_back(pair2); 109 | 110 | update_knowledge_client.call(updatePredSrv); 111 | } 112 | 113 | 114 | // helper function for feedback publishing 115 | void publishFeedback(int action_id, std::string fbstat) 116 | { 117 | rosplan_dispatch_msgs::ActionFeedback fb; 118 | fb.action_id = action_id; 119 | fb.status = fbstat; 120 | feedback_publisher_.publish(fb); 121 | } 122 | 123 | public: 124 | Quadrotor(ros::NodeHandle &nh, std::string &actionserver) : node_handle_(nh), message_store(nh) 125 | { 126 | // initialize knowledge client - we will be updating KMS throuch service calls 127 | update_knowledge_client = nh.serviceClient("/kcl_rosplan/update_knowledge_base"); 128 | 129 | // initialize motor client - this service is used for motor shutdown 130 | motor_service_client = nh.serviceClient("/shutdown"); 131 | 132 | // initialize velocity publisher - we will send movement commands (geometry_msgs::Twist messages) through this 133 | velocity_publisher_ = node_handle_.advertise("cmd_vel", 10); 134 | 135 | // feedback to the planning system will be send through this topic 136 | feedback_publisher_ = node_handle_.advertise("/kcl_rosplan/action_feedback", 10, true); 137 | 138 | height_ = -1; // should be updated in heightCallback 139 | } 140 | 141 | ~Quadrotor() 142 | { 143 | stop(); 144 | } 145 | 146 | // --- ACTION DISPATCH -- 147 | void dispatchCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr& msg) 148 | { 149 | ROS_INFO("Quadrotor: action recieved"); 150 | // parse action message 151 | std::string actionName = msg->name; 152 | int action_id = msg->action_id; 153 | 154 | if(actionName.compare("takeoff") == 0) 155 | { 156 | takeoff(action_id); 157 | } else if (actionName.compare("land") == 0) 158 | { 159 | land(action_id); 160 | } else if (actionName.compare("flysquare") == 0) 161 | { 162 | flysquare(action_id); 163 | } 164 | } 165 | 166 | void takeoff(int action_id) 167 | { 168 | publishFeedback(action_id,"action enabled"); 169 | 170 | ros::Rate loop_rate(0.25); 171 | ROS_INFO("Dispatching takeoff action."); 172 | // publish message to take off 173 | velocity_ = geometry_msgs::Twist(); 174 | velocity_.linear.z = 1.0; 175 | velocity_publisher_.publish(velocity_); 176 | 177 | loop_rate.sleep(); 178 | // publish message to stop 179 | stop(); 180 | ROS_INFO("Action takeoff dispatched."); 181 | 182 | // update knowledge base 183 | oneVariablePredicate("airborne","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE); 184 | oneVariablePredicate("grounded","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE); 185 | 186 | publishFeedback(action_id,"action achieved"); 187 | } 188 | 189 | void land(int action_id) 190 | { 191 | publishFeedback(action_id,"action enabled"); 192 | 193 | ROS_INFO("Dispatching land action."); 194 | // publish message to start descent 195 | velocity_ = geometry_msgs::Twist(); 196 | velocity_.linear.z = -1.0; 197 | velocity_publisher_.publish(velocity_); 198 | 199 | // wait until the drone touches the ground 200 | while(height_ > 0.2) 201 | { 202 | //ROS_INFO("Current height = %f", height_); 203 | 204 | // we need to repeat heightCallback to update height_ 205 | ros::spinOnce(); 206 | } 207 | 208 | // stop drone and shutdown motors 209 | emergency(); 210 | 211 | ROS_INFO("Action land dispatched."); 212 | 213 | // update knowledge base 214 | oneVariablePredicate("finished","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE); 215 | oneVariablePredicate("grounded","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE); 216 | oneVariablePredicate("airborne","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_KNOWLEDGE); 217 | 218 | publishFeedback(action_id,"action achieved"); 219 | } 220 | 221 | void flysquare(int action_id){ 222 | publishFeedback(action_id,"action enabled"); 223 | 224 | ros::Rate loop_rate(0.5); 225 | ROS_INFO("Dispatching flysquare action."); 226 | // forward 227 | velocity_ = geometry_msgs::Twist(); 228 | velocity_.linear.x = 1.0; 229 | velocity_publisher_.publish(velocity_); 230 | loop_rate.sleep(); 231 | 232 | // right 233 | velocity_ = geometry_msgs::Twist(); 234 | velocity_.linear.y = 1.0; 235 | velocity_publisher_.publish(velocity_); 236 | loop_rate.sleep(); 237 | 238 | // backward 239 | velocity_ = geometry_msgs::Twist(); 240 | velocity_.linear.x = -1.0; 241 | velocity_publisher_.publish(velocity_); 242 | loop_rate.sleep(); 243 | 244 | // left 245 | velocity_ = geometry_msgs::Twist(); 246 | velocity_.linear.y = -1.0; 247 | velocity_publisher_.publish(velocity_); 248 | loop_rate.sleep(); 249 | 250 | stop(); 251 | ROS_INFO("Action flysquare dispatched."); 252 | 253 | // update knowledge base 254 | oneVariablePredicate("squaredone","q","q1",rosplan_knowledge_msgs::KnowledgeUpdateService::Request::ADD_KNOWLEDGE); 255 | 256 | publishFeedback(action_id,"action achieved"); 257 | } 258 | 259 | // process messages received from sonar 260 | // -> range = 0.17 if landed 261 | void heightCallback(const sensor_msgs::Range::ConstPtr& msg) 262 | { 263 | // update quadrotor height 264 | height_ = msg->range; 265 | } 266 | 267 | void emergency() 268 | { 269 | // publish message to stop 270 | stop(); 271 | // shutdown motors 272 | shutdown(); 273 | } 274 | 275 | void stop() 276 | { 277 | if(velocity_publisher_.getNumSubscribers() > 0) 278 | { 279 | velocity_ = geometry_msgs::Twist(); 280 | velocity_publisher_.publish(velocity_); // sending stop message 281 | } 282 | } 283 | 284 | void shutdown() 285 | { 286 | // call shutdown service with empty service to shutdown motors 287 | std_srvs::Empty empty; 288 | motor_service_client.call(empty); 289 | } 290 | 291 | }; 292 | 293 | } // namespace rosplan_interface_quadrotor 294 | 295 | 296 | int main(int argc, char **argv) 297 | { 298 | ros::init(argc, argv, "rosplan_interface_quadrotor"); 299 | ros::NodeHandle nh; 300 | 301 | // initialize actionserver name by request to ROS parameter server 302 | std::string actionserver; 303 | nh.param("nav_server", actionserver, std::string("/nav_server")); 304 | 305 | rosplan_interface_quadrotor::Quadrotor quad_iface(nh,actionserver); 306 | 307 | ros::Subscriber action_sub = nh.subscribe("/kcl_rosplan/action_dispatch", 1000, &rosplan_interface_quadrotor::Quadrotor::dispatchCallback, &quad_iface); 308 | 309 | ros::Subscriber height_sub = nh.subscribe("/sonar_height", 1000, &rosplan_interface_quadrotor::Quadrotor::heightCallback, &quad_iface); 310 | 311 | ros::spin(); 312 | 313 | return 0; 314 | } 315 | --------------------------------------------------------------------------------