147 lines
5.6 KiB
C++
147 lines
5.6 KiB
C++
#ifndef DOCK_CALC_H
|
|
#define DOCK_CALC_H
|
|
|
|
#include <robot_nav_2d_utils/footprint.h>
|
|
#include <robot_geometry_msgs/PoseStamped.h>
|
|
|
|
#include <robot_nav_msgs/Path.h>
|
|
#include <robot_nav_msgs/OccupancyGrid.h>
|
|
#include <robot_nav_msgs/GetPlan.h>
|
|
|
|
#include <robot_costmap_2d/costmap_2d_robot.h>
|
|
#include <robot_costmap_2d/costmap_2d.h>
|
|
#include <robot_costmap_2d/inflation_layer.h>
|
|
|
|
#include <tf3/LinearMath/Quaternion.h>
|
|
|
|
#include "dock_planner/utils/curve_common.h"
|
|
#include "dock_planner/utils/pose.h"
|
|
#include "dock_planner/utils/common.h"
|
|
#include <robot/robot.h>
|
|
|
|
#include <vector>
|
|
|
|
using namespace std;
|
|
|
|
namespace dock_planner
|
|
{
|
|
struct RobotGeometry
|
|
{
|
|
double length; // dọc trục X
|
|
double width; // dọc trục Y
|
|
double radius; // circle around base_link
|
|
};
|
|
|
|
class DockCalc
|
|
{
|
|
private:
|
|
/* data */
|
|
Spline_Inf* input_spline_inf;
|
|
CurveCommon* CurveDesign;
|
|
|
|
vector<robot_geometry_msgs::Pose> posesOnPathWay;
|
|
vector<robot_geometry_msgs::Point> footprint_robot_;
|
|
robot_geometry_msgs::PoseStamped save_goal_;
|
|
// std::vector<robot_geometry_msgs::Pose> free_zone_;
|
|
|
|
bool check_goal_;
|
|
int store_start_nearest_idx_;
|
|
int start_target_idx_;
|
|
double min_distance_to_goal_;
|
|
|
|
// Tính khoảng cách giữa 2 điểm
|
|
inline double calc_distance(const robot_geometry_msgs::Pose& p1, const robot_geometry_msgs::Pose& p2)
|
|
{
|
|
return std::hypot(p1.position.x - p2.position.x, p1.position.y - p2.position.y);
|
|
}
|
|
|
|
inline double calculateAdaptiveStep(double segment_length, double desired_point_spacing = 0.02)
|
|
{
|
|
return desired_point_spacing / segment_length;
|
|
}
|
|
|
|
inline double calculateAngle(double x1, double y1, double x2, double y2)
|
|
{
|
|
double angle = atan2(y2 - y1, x2 - x1);
|
|
return angle;
|
|
}
|
|
|
|
inline double normalizeAngle(double angle)
|
|
{
|
|
while (angle > M_PI)
|
|
angle -= 2.0 * M_PI;
|
|
while (angle <= -M_PI)
|
|
angle += 2.0 * M_PI;
|
|
return angle;
|
|
}
|
|
|
|
double calculateNURBSLength(CurveCommon* curve_design,
|
|
Spline_Inf* spline_inf,
|
|
int degree,
|
|
double initial_step = 0.02);
|
|
|
|
robot_geometry_msgs::Pose offsetPoint(const robot_geometry_msgs::Pose& A,const double& theta, const double& d);
|
|
|
|
double pointToSegmentDistance(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
|
|
|
|
double signedAngle(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
|
|
|
|
double compute_t(const robot_geometry_msgs::Pose& A, const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C);
|
|
|
|
robot_geometry_msgs::Pose compute_D(const robot_geometry_msgs::Pose& B, const robot_geometry_msgs::Pose& C,const double& t);
|
|
|
|
void updatePoseOrientation(std::vector<robot_geometry_msgs::Pose>& saved_poses,
|
|
std::vector<robot_geometry_msgs::Pose>& nurbs_path,
|
|
bool reverse = true);
|
|
|
|
bool findPathTMP(const vector<robot_geometry_msgs::Pose>& control_points,
|
|
vector<robot_geometry_msgs::Pose>& saved_poses,
|
|
const int& degree, double step = 0.02);
|
|
|
|
bool findNURBSControlPoints(int& dir, vector<robot_geometry_msgs::Pose>& control_points,
|
|
const robot_geometry_msgs::Pose& start_pose,
|
|
const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
|
|
const double& angle_threshol, const int& degree);
|
|
|
|
int findNearestPointOnPath(int& dir, const robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Pose>& posesOnPathWay,
|
|
const double& angle_threshol);
|
|
|
|
bool isFreeSpace(const robot_geometry_msgs::Pose& pose, const std::vector<robot_geometry_msgs::Point>& footprint);
|
|
|
|
std::vector<robot_geometry_msgs::Point> calcFreeZone(const robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Point>& footprint);
|
|
|
|
std::vector<robot_geometry_msgs::Point> interpolateFootprint(const robot_geometry_msgs::Pose& pose,
|
|
const std::vector<robot_geometry_msgs::Point>& footprint, const double& resolution);
|
|
|
|
RobotGeometry computeGeometry(const std::vector<robot_geometry_msgs::Point>& fp);
|
|
|
|
|
|
public:
|
|
//Params
|
|
robot_costmap_2d::Costmap2D* costmap_;
|
|
|
|
int cost_threshold_; // Threshold for obstacle detection
|
|
double safety_distance_; // Safety distance from obstacles
|
|
double calib_safety_distance_;
|
|
|
|
DockCalc(/* args */);
|
|
|
|
~DockCalc();
|
|
|
|
bool makePlanMoveToDock(const robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
const vector<robot_geometry_msgs::Point>& footprint_robot,
|
|
const vector<robot_geometry_msgs::Point>& footprint_dock,
|
|
const int& degree, const double& step,
|
|
vector<robot_geometry_msgs::Pose>& planMoveToDock,
|
|
bool reverse = false);
|
|
|
|
|
|
};
|
|
|
|
}//namespace dock_planner
|
|
|
|
|
|
#endif |