From b31bb89c58d98ace6a6d7c49d16b146b093777d3 Mon Sep 17 00:00:00 2001 From: Guilherme Campos Camargo Date: Thu, 8 Sep 2016 13:54:25 -0300 Subject: [PATCH] Add Shift Avoidance The shift avoidance strategy uses as input a polar histogram representing the planar distances of the obstacles in the field of view of the vehicle. If any obstacle is detected between the vehicle and the taget, the vehicle moves a certain distance to the left before checking for obstacles again. If the path is safe, the vehicle continues to the target. Signed-off-by: Guilherme Campos Camargo --- .gitmodules | 1 + avoidance/CMakeLists.txt | 8 +- avoidance/QuadCopterShiftAvoidance.cc | 125 ++++++++++++++++++++++++++ avoidance/QuadCopterShiftAvoidance.hh | 38 ++++++++ common/math.cc | 24 ++--- samples/CMakeLists.txt | 32 ++++--- samples/coav_gcs.cc | 18 ++-- testbed/eeprom.bin | Bin 16384 -> 16384 bytes vehicles/MavQuadCopter.cc | 43 ++++----- vehicles/MavQuadCopter.hh | 9 +- 10 files changed, 234 insertions(+), 64 deletions(-) create mode 100644 avoidance/QuadCopterShiftAvoidance.cc create mode 100644 avoidance/QuadCopterShiftAvoidance.hh diff --git a/.gitmodules b/.gitmodules index 03a130f..dbc428c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,4 @@ [submodule "modules/mavlink_vehicles"] path = modules/mavlink_vehicles url = https://github.com/01org/mavlink-vehicles + branch = master diff --git a/avoidance/CMakeLists.txt b/avoidance/CMakeLists.txt index f08979c..882db8a 100644 --- a/avoidance/CMakeLists.txt +++ b/avoidance/CMakeLists.txt @@ -1,9 +1,15 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) set (sources + QuadCopterShiftAvoidance.cc QuadCopterVFFAvoidance.cc ) add_library(avoidance ${sources}) -target_link_libraries(avoidance common ${GAZEBO_libraries} ${LDFLAGS}) +target_link_libraries(avoidance + common + vehicles + ${GAZEBO_libraries} + ${LDFLAGS} +) diff --git a/avoidance/QuadCopterShiftAvoidance.cc b/avoidance/QuadCopterShiftAvoidance.cc new file mode 100644 index 0000000..fd09153 --- /dev/null +++ b/avoidance/QuadCopterShiftAvoidance.cc @@ -0,0 +1,125 @@ +/* +// Copyright (c) 2016 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +*/ +#include "QuadCopterShiftAvoidance.hh" +#include "vehicles/MavQuadCopter.hh" +#include "common/common.hh" +#include "common/math.hh" +#include "glm/glm.hpp" + +#include +#include +#include +#include +#include + +namespace defaults +{ +const double safe_distance = 8.0; +const double lowest_altitude = 1.0; +const double detour_wp_angle = M_PI/3.0; +} + +void QuadCopterShiftAvoidance::avoid(const std::vector &histogram, + std::shared_ptr vehicle) +{ + Pose vehicle_pose = vehicle->vehicle_pose(); + + if (!vehicle->mav->is_ready()) { + return; + } + + vehicle->mav->set_autorotate_during_mission(true); + vehicle->mav->set_autorotate_during_detour(false); + + switch (this->avoidance_state) { + case avoid_state::detouring: { + // The vehicle is detouring. We need to wait for it to finish the + // detour before checking for obstacles again. + + if(vehicle->detour_finished()) { + this->avoidance_state = avoid_state::moving; + std::cout << "[avoid] state = moving..." << std::endl; + } + + break; + } + case avoid_state::moving: { + // The vehicle is moving to the target. We need to detect if an + // obstacle is found in order to start a detour. + + // The histogram does not have valid data. We're blind. + if (histogram.size() == 0) { + break; + } + + // Check if the path in front of the vehicle is safe + if (histogram[0] == 0 || histogram[0] > defaults::safe_distance) { + break; + } + + // Check if the current mission waypoint is closer than the closest obstacle + if (mavlink_vehicles::math::ground_dist( + vehicle->mav->get_global_position_int(), + vehicle->mav->get_mission_waypoint()) <= histogram[0]) { + break; + } + + // Check if we are too close to ground + if (vehicle_pose.pos.z < defaults::lowest_altitude) { + break; + } + + // Calculate the lookat vector + glm::dvec3 view_dir = + glm::dvec3(-sin(vehicle_pose.yaw()), cos(vehicle_pose.yaw()), 0); + + // Calculate the direction of the detour waypoint (horizontal rotation + // around lookat vector) + glm::dvec3 wp_dir = + glm::dvec3(view_dir.x * cos(defaults::detour_wp_angle) - + view_dir.y * sin(defaults::detour_wp_angle), + view_dir.x * sin(defaults::detour_wp_angle) + + view_dir.y * cos(defaults::detour_wp_angle), + 0.0); + + // Calculate the global position of the waypoint + Pose wp = + Pose{vehicle_pose.pos + glm::abs(2.0) * glm::normalize(wp_dir), + glm::dquat(0, 0, 0, 0)}; + + // Send the detour waypoint to the vehicle + vehicle->set_target_pose(wp); + + // Update avoidance state + this->avoidance_state = avoid_state::detouring; + + // Print info + std::cout << "[avoid] state = detouring..." << std::endl; + + std::cout << "[avoid] wp (x, y, z): " << wp.pos.x << ", " << wp.pos.y + << "," << wp.pos.z << std::endl; + + std::cout << "[avoid] vh (x, y, z): " << vehicle_pose.pos.x << ", " + << vehicle_pose.pos.y << "," << vehicle_pose.pos.z + << std::endl; + + break; + } + } + + return; +} + diff --git a/avoidance/QuadCopterShiftAvoidance.hh b/avoidance/QuadCopterShiftAvoidance.hh new file mode 100644 index 0000000..8cbf4b2 --- /dev/null +++ b/avoidance/QuadCopterShiftAvoidance.hh @@ -0,0 +1,38 @@ +/* +// Copyright (c) 2016 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +*/ +#pragma once + +#include "common/common.hh" +#include "vehicles/MavQuadCopter.hh" + +#include +#include +#include + +class QuadCopterShiftAvoidance +{ + public: + void avoid(const std::vector &histogram, + std::shared_ptr vehicle); + + private: + std::chrono::time_point wp_sent_time = + std::chrono::system_clock::from_time_t(0); + + enum class avoid_state { moving, detouring }; + avoid_state avoidance_state = avoid_state::moving; +}; + diff --git a/common/math.cc b/common/math.cc index 041143c..c11e1b2 100644 --- a/common/math.cc +++ b/common/math.cc @@ -35,16 +35,20 @@ Pose operator-(const Pose &a, const Pose &b) void Pose::set_rot(float pitch, float roll, float yaw) { - double phi, the, psi; - - phi = roll / 2.0; - the = pitch / 2.0; - psi = yaw / 2.0; - - this->rot.w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); - this->rot.x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); - this->rot.y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); - this->rot.z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); + double phi, the, psi; + + phi = roll / 2.0; + the = pitch / 2.0; + psi = yaw / 2.0; + + this->rot.w = + cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi); + this->rot.x = + sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi); + this->rot.y = + cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi); + this->rot.z = + cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi); } double Pose::pitch() diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index db10904..c6c7188 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -3,18 +3,28 @@ cmake_minimum_required(VERSION 2.8 FATAL_ERROR) add_executable(coav_gcs coav_gcs.cc) add_executable(test_detector test.cc) -target_link_libraries(coav_gcs common sensors detection avoidance utils vehicles ${GAZEBO_LIBRARIES} ${PROTOBUF_LIBRARIES} ${LDFLAGS}) +target_link_libraries(coav_gcs + common + sensors + detection + avoidance + utils + vehicles + ${GAZEBO_LIBRARIES} + ${PROTOBUF_LIBRARIES} + ${LDFLAGS} +) target_link_libraries(test_detector - common - communication - detection - utils - sensors - vehicles - ${GAZEBO_LIBRARIES} - ${PROTOBUF_LIBRARIES} - ${OPENGL_LIBRARIES} - ${GLUT_LIBRARIES} + common + communication + detection + utils + sensors + vehicles + ${GAZEBO_LIBRARIES} + ${PROTOBUF_LIBRARIES} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARIES} ) diff --git a/samples/coav_gcs.cc b/samples/coav_gcs.cc index 4c91a09..d496dbe 100644 --- a/samples/coav_gcs.cc +++ b/samples/coav_gcs.cc @@ -23,11 +23,11 @@ #include #include -#include "avoidance/QuadCopterVFFAvoidance.hh" +#include "avoidance/QuadCopterShiftAvoidance.hh" #include "vehicles/GazeboQuadCopter.hh" #include "vehicles/MavQuadCopter.hh" #include "sensors/GazeboRealSenseCamera.hh" -#include "detection/DepthImageObstacleDetector.hh" +#include "detection/DepthImagePolarHistDetector.hh" #include "common/common.hh" enum device_type_ { GAZEBO, PHYSICAL, OTHER }; @@ -134,8 +134,8 @@ int main(int argc, char **argv) } std::shared_ptr depth_camera; - std::shared_ptr vehicle; - std::shared_ptr obstacle_detector; + std::shared_ptr vehicle; + std::shared_ptr obstacle_detector; // Initialize Sensors switch (depth_camera_type) { @@ -144,12 +144,13 @@ int main(int argc, char **argv) case GAZEBO: depth_camera = std::make_shared(); std::cout << "[coav] GazeboRealSenseCamera instantiated" << std::endl; - obstacle_detector = std::make_shared(depth_camera); + obstacle_detector = std::make_shared( + depth_camera, 180.0 * (atan(depth_camera->get_fov_tan())) / M_PI); std::cout << "[coav] ObstacleDetector instantiated" << std::endl; } // Initialize Avoidance Strategy - auto avoidance = std::make_shared(); + auto avoidance = std::make_shared(); // Initialize Vehicle switch (depth_camera_type) { @@ -162,9 +163,10 @@ int main(int argc, char **argv) while (running) { // Sense - std::vector obstacles = obstacle_detector->detect(); + std::vector obstacles; + std::vector hist = obstacle_detector->detect(); // Avoid - avoidance->avoid(obstacles, vehicle); + avoidance->avoid(hist, vehicle); } } diff --git a/testbed/eeprom.bin b/testbed/eeprom.bin index f6b1f5be1085053324e05e6ea0dc3b1d251160fe..e410bc0d62eb90a89737a9254e0e6c87c3b27f2d 100755 GIT binary patch delta 121 zcmZo@U~Fh$oS??Yv{8K-qf3bsBZCM-i4y|@BSQ%j69X5J1dAMEU|?dX1thread_run = true; this->thread = std::thread(&MavQuadCopter::run, this); - this->thread.detach(); } MavQuadCopter::~MavQuadCopter() @@ -88,7 +87,7 @@ void MavQuadCopter::run() // Check if vehicle is ready if (!this->mav->is_ready()) { - return; + continue; } mavlink_vehicles::arm_status arm_stat = this->mav->get_arm_status(); @@ -104,34 +103,34 @@ void MavQuadCopter::run() this->vehicle_state = ACTIVE_AIRBORNE; } - break; + continue; } case INIT_ON_GROUND: { if (!this->autotakeoff) { this->vehicle_state = ACTIVE_ON_GROUND; - break; + continue; } // Execute takeoff procedures if (mode != mavlink_vehicles::mode::GUIDED) { this->mav->set_mode(mavlink_vehicles::mode::GUIDED); - break; + continue; } if (arm_stat != mavlink_vehicles::arm_status::ARMED) { this->mav->arm_throttle(); - break; + continue; } if (status != mavlink_vehicles::status::ACTIVE) { this->mav->takeoff(); - break; + continue; } // Takeoff succeeded this->vehicle_state = ACTIVE_AIRBORNE; - break; + continue; } case ACTIVE_ON_GROUND: { @@ -139,29 +138,12 @@ void MavQuadCopter::run() this->vehicle_state = ACTIVE_AIRBORNE; } - break; + continue; } case ACTIVE_AIRBORNE: { - // Do not send detour waypoint if it hasn't changed - if (mavlink_vehicles::math::dist(curr_detour_wp, prev_detour_wp) < - defaults::wp_equal_dist_m) { - break; - } - - // Detour wp has changed - prev_detour_wp = curr_detour_wp; - - // TODO: Rotate if autorotate is enabled - // if (this->autorotate) { - // } - - // Send detour wp - this->mav->send_detour_waypoint(curr_detour_wp); - - break; + continue; } - } } } @@ -218,7 +200,7 @@ void MavQuadCopter::set_target_pose(Pose pose) mavlink_vehicles::local_pos(pose.pos.y, pose.pos.x, -pose.pos.z), this->mav->get_home_position_int()); - this->mav->send_detour_waypoint(global_coord, true, false); + this->mav->send_detour_waypoint(global_coord); } void MavQuadCopter::rotate(double angle_deg) @@ -226,3 +208,8 @@ void MavQuadCopter::rotate(double angle_deg) this->mav->rotate(angle_deg); } +bool MavQuadCopter::detour_finished() +{ + return this->mav->get_mode() == mavlink_vehicles::mode::AUTO; +} + diff --git a/vehicles/MavQuadCopter.hh b/vehicles/MavQuadCopter.hh index 608848e..bf7a4cd 100644 --- a/vehicles/MavQuadCopter.hh +++ b/vehicles/MavQuadCopter.hh @@ -37,12 +37,13 @@ class MavQuadCopter : public QuadCopter Pose vehicle_pose() override; void set_target_pose(Pose pose) override; void rotate(double angle_deg); - - private: + bool detour_finished(); // Mavlink vehicle std::shared_ptr mav; + private: + // Connection int sock = 0; socklen_t fromlen = {0}; @@ -59,9 +60,5 @@ class MavQuadCopter : public QuadCopter vstate vehicle_state = INIT; bool autotakeoff = false; - bool autorotate = false; - - mavlink_vehicles::global_pos_int curr_detour_wp; - mavlink_vehicles::global_pos_int prev_detour_wp; };