forked from ai-winter/ros_motion_planning
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
update sfm,, they move, but still some bugs
- Loading branch information
Showing
19 changed files
with
1,641 additions
and
91 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/third_party/dynamic_xml_config/main_generate.py user_config_orca.yaml | ||
| python ../src/third_party/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.