-
Notifications
You must be signed in to change notification settings - Fork 53
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
25 changed files
with
3,444 additions
and
2,715 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 |
---|---|---|
@@ -0,0 +1,24 @@ | ||
name: ci | ||
|
||
on: | ||
push: | ||
branches: | ||
- master | ||
pull_request: | ||
branches: | ||
- master | ||
|
||
jobs: | ||
build: | ||
runs-on: ubuntu-20.04 | ||
steps: | ||
- name: Checkout | ||
uses: actions/checkout@v3 | ||
- name: Setup ROS | ||
uses: ros-tooling/[email protected] | ||
with: | ||
required-ros-distributions: noetic | ||
- name: Build | ||
uses: ros-tooling/[email protected] | ||
with: | ||
target-ros1-distro: noetic |
This file was deleted.
Oops, something went wrong.
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 |
---|---|---|
@@ -1,53 +1,56 @@ | ||
#ifndef __LOOKUP_TABLE_GENERATOR_H | ||
#define __LOOKUP_TABLE_GENERATOR_H | ||
// Copyright 2019 amsl | ||
|
||
#ifndef STATE_LATTICE_PLANNER_LOOKUP_TABLE_GENERATOR_H | ||
#define STATE_LATTICE_PLANNER_LOOKUP_TABLE_GENERATOR_H | ||
|
||
#include <iostream> | ||
#include <fstream> | ||
#include <iostream> | ||
#include <string> | ||
|
||
#include <ros/ros.h> | ||
|
||
#include <Eigen/Dense> | ||
|
||
#include "state_lattice_planner/lookup_table_utils.h" | ||
#include "trajectory_generator/motion_model_diff_drive.h" | ||
#include "trajectory_generator/trajectory_generator_diff_drive.h" | ||
#include "state_lattice_planner/lookup_table_utils.h" | ||
|
||
class LookupTableGenerator | ||
{ | ||
public: | ||
LookupTableGenerator(void); | ||
LookupTableGenerator(void); | ||
|
||
std::string process(void); | ||
void save(std::string&); | ||
double get_target_velocity(const Eigen::Vector3d&); | ||
std::string process(void); | ||
void save(std::string&); | ||
double get_target_velocity(const Eigen::Vector3d&); | ||
|
||
private: | ||
double MIN_X; | ||
double MAX_X; | ||
double DELTA_X; | ||
double MAX_Y; | ||
double DELTA_Y; | ||
double MAX_YAW; | ||
double DELTA_YAW; | ||
std::string LOOKUP_TABLE_FILE_NAME; | ||
double TARGET_VELOCITY; | ||
double MIN_V; | ||
double MAX_V; | ||
double DELTA_V; | ||
double MAX_KAPPA; | ||
double DELTA_KAPPA; | ||
double MAX_ACCELERATION; | ||
double MAX_YAWRATE; | ||
double MAX_D_YAWRATE; | ||
double MAX_WHEEL_ANGULAR_VELOCITY; | ||
double WHEEL_RADIUS; | ||
double TREAD; | ||
|
||
ros::NodeHandle nh; | ||
ros::NodeHandle local_nh; | ||
|
||
LookupTableUtils::LookupTable lookup_table; | ||
bool use_existing_lookup_table; | ||
double MIN_X; | ||
double MAX_X; | ||
double DELTA_X; | ||
double MAX_Y; | ||
double DELTA_Y; | ||
double MAX_YAW; | ||
double DELTA_YAW; | ||
std::string LOOKUP_TABLE_FILE_NAME; | ||
double TARGET_VELOCITY; | ||
double MIN_V; | ||
double MAX_V; | ||
double DELTA_V; | ||
double MAX_KAPPA; | ||
double DELTA_KAPPA; | ||
double MAX_ACCELERATION; | ||
double MAX_YAWRATE; | ||
double MAX_D_YAWRATE; | ||
double MAX_WHEEL_ANGULAR_VELOCITY; | ||
double WHEEL_RADIUS; | ||
double TREAD; | ||
|
||
ros::NodeHandle nh; | ||
ros::NodeHandle local_nh; | ||
|
||
LookupTableUtils::LookupTable lookup_table; | ||
bool use_existing_lookup_table; | ||
}; | ||
|
||
#endif// __LOOKUP_TABLE_GENERATOR_H | ||
#endif // STATE_LATTICE_PLANNER_LOOKUP_TABLE_GENERATOR_H |
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,31 +1,38 @@ | ||
#ifndef __LOOKUP_TABLE_UTILS_H | ||
#define __LOOKUP_TABLE_UTILS_H | ||
// Copyright 2019 amsl | ||
|
||
#include <iostream> | ||
#ifndef STATE_LATTICE_PLANNER_LOOKUP_TABLE_UTILS_H | ||
#define STATE_LATTICE_PLANNER_LOOKUP_TABLE_UTILS_H | ||
|
||
#include <Eigen/Dense> | ||
#include <fstream> | ||
#include <vector> | ||
#include <iostream> | ||
#include <map> | ||
#include <Eigen/Dense> | ||
#include <string> | ||
#include <vector> | ||
|
||
#include "trajectory_generator/motion_model_diff_drive.h" | ||
|
||
namespace LookupTableUtils | ||
{ | ||
class StateWithControlParams | ||
{ | ||
public: | ||
StateWithControlParams(void); | ||
class StateWithControlParams | ||
{ | ||
public: | ||
StateWithControlParams(void); | ||
|
||
Eigen::Vector3d state; // x, y, yaw | ||
MotionModelDiffDrive::ControlParams control; | ||
|
||
Eigen::Vector3d state;// x, y, yaw | ||
MotionModelDiffDrive::ControlParams control; | ||
private: | ||
}; | ||
private: | ||
}; | ||
|
||
typedef std::map<double, std::map<double, std::vector<StateWithControlParams> > > LookupTable; | ||
typedef std::map<double, std::map<double, std::vector<StateWithControlParams>>> | ||
LookupTable; | ||
|
||
bool load_lookup_table(const std::string&, LookupTable&); | ||
bool load_lookup_table(const std::string&, LookupTable&); | ||
|
||
void get_optimized_param_from_lookup_table(const LookupTable&, const Eigen::Vector3d, const double, const double, MotionModelDiffDrive::ControlParams&); | ||
} | ||
void get_optimized_param_from_lookup_table( | ||
const LookupTable&, const Eigen::Vector3d, const double, const double, | ||
MotionModelDiffDrive::ControlParams&); | ||
} // namespace LookupTableUtils | ||
|
||
#endif// __LOOKUP_TABLE_UTILS_H | ||
#endif // STATE_LATTICE_PLANNER_LOOKUP_TABLE_UTILS_H |
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,85 +1,99 @@ | ||
#ifndef __OBSTACLE_MAP_H | ||
#define __OBSTACLE_MAP_H | ||
// Copyright 2019 amsl | ||
|
||
#ifndef STATE_LATTICE_PLANNER_OBSTACLE_MAP_H | ||
#define STATE_LATTICE_PLANNER_OBSTACLE_MAP_H | ||
|
||
#include <iostream> | ||
#include <vector> | ||
|
||
namespace state_lattice_planner | ||
{ | ||
// | ||
// x | ||
// ^ | ||
// | height | ||
// | | ||
// y <- - - | ||
// width | ||
// | ||
|
||
// | ||
// x | ||
// ^ | ||
// | height | ||
// | | ||
// y <- - - | ||
// width | ||
// | ||
|
||
template<typename ELEMENT_TYPE> | ||
template <typename ELEMENT_TYPE> | ||
class ObstacleMap | ||
{ | ||
public: | ||
ObstacleMap(void); | ||
unsigned int get_index_from_xy(double, double) const; | ||
void set_shape(unsigned int, unsigned int, double); | ||
double get_resolution(void) const; | ||
ObstacleMap(void); | ||
unsigned int get_index_from_xy(double, double) const; | ||
void set_shape(unsigned int, unsigned int, double); | ||
double get_resolution(void) const; | ||
|
||
std::vector<ELEMENT_TYPE> data; | ||
|
||
std::vector<ELEMENT_TYPE> data; | ||
protected: | ||
unsigned int width;// cells | ||
unsigned int height;// cells | ||
double resolution;// m/cell | ||
double origin_x;// m | ||
double origin_y;// m | ||
unsigned int width; // cells | ||
unsigned int height; // cells | ||
double resolution; // m/cell | ||
double origin_x; // m | ||
double origin_y; // m | ||
}; | ||
|
||
template<typename ELEMENT_TYPE> | ||
template <typename ELEMENT_TYPE> | ||
ObstacleMap<ELEMENT_TYPE>::ObstacleMap(void) | ||
{ | ||
width = 0; | ||
height = 0; | ||
resolution = 0.0; | ||
origin_x = 0.0; | ||
origin_y = 0.0; | ||
data.clear(); | ||
width = 0; | ||
height = 0; | ||
resolution = 0.0; | ||
origin_x = 0.0; | ||
origin_y = 0.0; | ||
data.clear(); | ||
} | ||
|
||
template<typename ELEMENT_TYPE> | ||
unsigned int ObstacleMap<ELEMENT_TYPE>::get_index_from_xy(double x, double y) const | ||
template <typename ELEMENT_TYPE> | ||
unsigned int ObstacleMap<ELEMENT_TYPE>::get_index_from_xy(double x, | ||
double y) const | ||
{ | ||
unsigned int index = 0; | ||
index = std::round((x - origin_x) / resolution) + std::round((y - origin_y) / resolution) * height; | ||
return index; | ||
unsigned int index = 0; | ||
index = std::round((x - origin_x) / resolution) + | ||
std::round((y - origin_y) / resolution) * height; | ||
return index; | ||
} | ||
|
||
template<typename ELEMENT_TYPE> | ||
void ObstacleMap<ELEMENT_TYPE>::set_shape(unsigned int width_, unsigned int height_, double resolution_) | ||
template <typename ELEMENT_TYPE> | ||
void ObstacleMap<ELEMENT_TYPE>::set_shape(unsigned int width_, | ||
unsigned int height_, | ||
double resolution_) | ||
{ | ||
if(width_ > 0 && height_ > 0){ | ||
width = width_; | ||
height = height_; | ||
}else{ | ||
std::cout << "\033[31mvalue error: width and height must be > 0\033[0m" << std::endl; | ||
return; | ||
} | ||
if(resolution_ > 0.0){ | ||
resolution = resolution_; | ||
}else{ | ||
std::cout << "\033[31mvalue error: resolution must be > 0.0 m\033[0m" << std::endl; | ||
return; | ||
} | ||
// origin is in the lower right-hand corner of the map. | ||
origin_x = -static_cast<double>(height) * 0.5 * resolution; | ||
origin_y = -static_cast<double>(width) * 0.5 * resolution; | ||
if (width_ > 0 && height_ > 0) | ||
{ | ||
width = width_; | ||
height = height_; | ||
} | ||
else | ||
{ | ||
std::cout << "\033[31mvalue error: width and height must be > 0\033[0m" | ||
<< std::endl; | ||
return; | ||
} | ||
if (resolution_ > 0.0) | ||
{ | ||
resolution = resolution_; | ||
} | ||
else | ||
{ | ||
std::cout << "\033[31mvalue error: resolution must be > 0.0 m\033[0m" | ||
<< std::endl; | ||
return; | ||
} | ||
// origin is in the lower right-hand corner of the map. | ||
origin_x = -static_cast<double>(height) * 0.5 * resolution; | ||
origin_y = -static_cast<double>(width) * 0.5 * resolution; | ||
} | ||
|
||
template<typename ELEMENT_TYPE> | ||
template <typename ELEMENT_TYPE> | ||
double ObstacleMap<ELEMENT_TYPE>::get_resolution(void) const | ||
{ | ||
return resolution; | ||
return resolution; | ||
} | ||
|
||
} | ||
} // namespace state_lattice_planner | ||
|
||
#endif// __OBSTACLE_MAP_H | ||
#endif // STATE_LATTICE_PLANNER_OBSTACLE_MAP_H |
Oops, something went wrong.