-
Notifications
You must be signed in to change notification settings - Fork 407
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'multi_agent_dev' into zhanyu.guo/master
- Loading branch information
Showing
18 changed files
with
1,631 additions
and
87 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,6 +1,6 @@ | ||
| gnome-terminal -- bash -c "./orca.sh" | ||
| gnome-terminal -- bash -c "./multi.sh" | ||
|
|
||
| echo "Waiting for initialization (sleep 15s)..." | ||
| sleep 15 | ||
| source ../devel/setup.bash | ||
| rosrun orca_planner goal_publisher.py | ||
| rosrun sim_env goal_publisher.py |
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,3 @@ | ||
| source ../devel/setup.bash | ||
| python ../src/plugins/dynamic_xml_config/main_generate.py user_config_orca.yaml | ||
| python ../src/plugins/dynamic_xml_config/main_generate.py user_config_multi.yaml | ||
| roslaunch sim_env main.launch |
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
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,35 @@ | ||
| cmake_minimum_required(VERSION 3.0.2) | ||
| project(sfm_planner) | ||
|
|
||
| find_package(catkin REQUIRED COMPONENTS | ||
| angles | ||
| costmap_2d | ||
| geometry_msgs | ||
| nav_core | ||
| nav_msgs | ||
| navfn | ||
| pluginlib | ||
| roscpp | ||
| tf2_geometry_msgs | ||
| tf2_ros | ||
| base_local_planner | ||
| local_planner | ||
| ) | ||
|
|
||
| catkin_package( | ||
| INCLUDE_DIRS include | ||
| CATKIN_DEPENDS local_planner | ||
| ) | ||
|
|
||
| include_directories( | ||
| include | ||
| ${catkin_INCLUDE_DIRS} | ||
| ) | ||
|
|
||
| add_library(${PROJECT_NAME} | ||
| src/sfm_planner.cpp | ||
| ) | ||
|
|
||
| target_link_libraries(${PROJECT_NAME} | ||
| ${catkin_LIBRARIES} | ||
| ) |
204 changes: 204 additions & 0 deletions
204
src/core/local_planner/sfm_planner/include/lightsfm/angle.hpp
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,204 @@ | ||
| /***********************************************************************/ | ||
| /** */ | ||
| /** angle.hpp */ | ||
| /** */ | ||
| /** Copyright (c) 2016, Service Robotics Lab. */ | ||
| /** http://robotics.upo.es */ | ||
| /** */ | ||
| /** All rights reserved. */ | ||
| /** */ | ||
| /** Authors: */ | ||
| /** Ignacio Perez-Hurtado (maintainer) */ | ||
| /** Jesus Capitan */ | ||
| /** Fernando Caballero */ | ||
| /** Luis Merino */ | ||
| /** */ | ||
| /** This software may be modified and distributed under the terms */ | ||
| /** of the BSD license. See the LICENSE file for details. */ | ||
| /** */ | ||
| /** http://www.opensource.org/licenses/BSD-3-Clause */ | ||
| /** */ | ||
| /***********************************************************************/ | ||
|
|
||
| #ifndef _ANGLE_HPP_ | ||
| #define _ANGLE_HPP_ | ||
|
|
||
| #include <iostream> | ||
| #include <cmath> | ||
|
|
||
| namespace utils | ||
| { | ||
| class Angle | ||
| { | ||
| public: | ||
| enum AngleRange | ||
| { | ||
| // [0, 2*pi) or [0°, 360°) | ||
| PositiveOnlyRange, | ||
| // (-pi, +pi] or (-180°, 180°] | ||
| PositiveNegativeRange | ||
| }; | ||
|
|
||
| Angle() : value(0) | ||
| { | ||
| } | ||
| virtual ~Angle() | ||
| { | ||
| } | ||
|
|
||
| static Angle fromRadian(double value) | ||
| { | ||
| return Angle(value); | ||
| } | ||
|
|
||
| static Angle fromDegree(double value) | ||
| { | ||
| return Angle(value / 180 * M_PI); | ||
| } | ||
|
|
||
| double toRadian(AngleRange range = PositiveNegativeRange) const | ||
| { | ||
| if (range == PositiveNegativeRange) | ||
| { | ||
| return value; | ||
| } | ||
| else | ||
| { | ||
| return (value >= 0) ? value : (value + 2 * M_PI); | ||
| } | ||
| } | ||
|
|
||
| double cos() const | ||
| { | ||
| return std::cos(value); | ||
| } | ||
|
|
||
| double sin() const | ||
| { | ||
| return std::sin(value); | ||
| } | ||
|
|
||
| double toDegree(AngleRange range = PositiveNegativeRange) const | ||
| { | ||
| double degreeValue = value * 180 / M_PI; | ||
| if (range == PositiveNegativeRange) | ||
| { | ||
| return degreeValue; | ||
| } | ||
| else | ||
| { | ||
| return (degreeValue >= 0) ? degreeValue : (degreeValue + 360); | ||
| } | ||
| } | ||
|
|
||
| void setRadian(double value) | ||
| { | ||
| Angle::value = value; | ||
| normalize(); | ||
| } | ||
|
|
||
| void setDegree(double value) | ||
| { | ||
| Angle::value = value / 180 * M_PI; | ||
| normalize(); | ||
| } | ||
|
|
||
| int sign() const | ||
| { | ||
| if (value == 0) | ||
| { | ||
| return 0; | ||
| } | ||
| else if (value > 0) | ||
| { | ||
| return 1; | ||
| } | ||
| else | ||
| { | ||
| return -1; | ||
| } | ||
| } | ||
|
|
||
| Angle operator+(const Angle &other) const | ||
| { | ||
| return Angle(value + other.value); | ||
| } | ||
|
|
||
| Angle operator-(const Angle &other) const | ||
| { | ||
| return Angle(value - other.value); | ||
| } | ||
|
|
||
| Angle &operator+=(const Angle &other) | ||
| { | ||
| value += other.value; | ||
| normalize(); | ||
| return *this; | ||
| } | ||
|
|
||
| Angle &operator-=(const Angle &other) | ||
| { | ||
| value -= other.value; | ||
| normalize(); | ||
| return *this; | ||
| } | ||
|
|
||
| bool operator==(const Angle &other) const | ||
| { | ||
| return value == other.value; | ||
| } | ||
|
|
||
| bool operator!=(const Angle &other) const | ||
| { | ||
| return value != other.value; | ||
| } | ||
|
|
||
| bool operator<(const Angle &other) const | ||
| { | ||
| return value < other.value; | ||
| } | ||
|
|
||
| bool operator<=(const Angle &other) const | ||
| { | ||
| return value <= other.value; | ||
| } | ||
|
|
||
| bool operator>(const Angle &other) const | ||
| { | ||
| return value > other.value; | ||
| } | ||
|
|
||
| bool operator>=(const Angle &other) const | ||
| { | ||
| return value >= other.value; | ||
| } | ||
|
|
||
| private: | ||
| Angle(double value) | ||
| { | ||
| Angle::value = value; | ||
| normalize(); | ||
| } | ||
|
|
||
| void normalize() | ||
| { | ||
| while (value <= -M_PI) | ||
| value += 2 * M_PI; | ||
| while (value > M_PI) | ||
| value -= 2 * M_PI; | ||
| } | ||
|
|
||
| double value; | ||
| }; | ||
| } // namespace utils | ||
|
|
||
| namespace std | ||
| { | ||
| inline ostream &operator<<(ostream &stream, const utils::Angle &alpha) | ||
| { | ||
| stream << alpha.toRadian(); | ||
| return stream; | ||
| } | ||
| } // namespace std | ||
|
|
||
| #endif |
Oops, something went wrong.