This repository has been archived by the owner on Aug 5, 2022. It is now read-only.
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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 <guilherme.campos.camargo@intel.com>
- Loading branch information
Showing
10 changed files
with
234 additions
and
64 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,3 +1,4 @@ | ||
| [submodule "modules/mavlink_vehicles"] | ||
| path = modules/mavlink_vehicles | ||
| url = https://github.com/01org/mavlink-vehicles | ||
| branch = master |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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} | ||
| ) | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <chrono> | ||
| #include <memory> | ||
| #include <vector> | ||
| #include <limits> | ||
| #include <iostream> | ||
|
|
||
| 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<double> &histogram, | ||
| std::shared_ptr<MavQuadCopter> 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; | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <chrono> | ||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| class QuadCopterShiftAvoidance | ||
| { | ||
| public: | ||
| void avoid(const std::vector<double> &histogram, | ||
| std::shared_ptr<MavQuadCopter> vehicle); | ||
|
|
||
| private: | ||
| std::chrono::time_point<std::chrono::system_clock> wp_sent_time = | ||
| std::chrono::system_clock::from_time_t(0); | ||
|
|
||
| enum class avoid_state { moving, detouring }; | ||
| avoid_state avoidance_state = avoid_state::moving; | ||
| }; | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Binary file not shown.
Oops, something went wrong.