Skip to content

Commit

Permalink
Fix format
Browse files Browse the repository at this point in the history
  • Loading branch information
@Taka-Kazu
Taka-Kazu committed Nov 30, 2023
1 parent 9ecdc07 commit 9c4aaa9
Show file tree
Hide file tree
Showing 21 changed files with 3,409 additions and 2,687 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,7 @@ target_link_libraries(trajectory_viewer

if(CATKIN_ENABLE_TESTING)
find_package(roslint REQUIRED)
set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references")
roslint_cpp()
roslint_add_test()

Expand Down
73 changes: 38 additions & 35 deletions include/state_lattice_planner/lookup_table_generator.h
View file
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
43 changes: 25 additions & 18 deletions include/state_lattice_planner/lookup_table_utils.h
View file
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
126 changes: 70 additions & 56 deletions include/state_lattice_planner/obstacle_map.h
View file
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
Loading

0 comments on commit 9c4aaa9

Please sign in to comment.