You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
64 lines
1.5 KiB
64 lines
1.5 KiB
#ifndef APF_H
|
|
#define APF_H
|
|
|
|
#include <ros/ros.h>
|
|
#include <Eigen/Eigen>
|
|
#include <iostream>
|
|
#include <algorithm>
|
|
#include <iostream>
|
|
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
#include <std_msgs/Empty.h>
|
|
#include <sensor_msgs/PointCloud2.h>
|
|
#include <nav_msgs/Odometry.h>
|
|
|
|
#include <pcl/point_cloud.h>
|
|
#include <pcl/point_types.h>
|
|
#include <pcl_conversions/pcl_conversions.h>
|
|
|
|
#include "local_planner_alg.h"
|
|
#include "local_planner_utils.h"
|
|
|
|
using namespace std;
|
|
|
|
|
|
class APF : public local_planner_alg
|
|
{
|
|
private:
|
|
// 参数
|
|
double inflate_distance;
|
|
double sensor_max_range;
|
|
double max_att_dist;
|
|
double k_repulsion;
|
|
double k_attraction;
|
|
double min_dist;
|
|
double ground_height;
|
|
double ground_safe_height;
|
|
double safe_distance;
|
|
|
|
bool has_local_map_;
|
|
bool has_odom_;
|
|
|
|
Eigen::Vector3d repulsive_force;
|
|
Eigen::Vector3d attractive_force;
|
|
|
|
pcl::PointCloud<pcl::PointXYZ> latest_local_pcl_;
|
|
sensor_msgs::PointCloud2ConstPtr local_map_ptr_;
|
|
nav_msgs::Odometry current_odom;
|
|
|
|
public:
|
|
virtual void set_odom(nav_msgs::Odometry cur_odom);
|
|
virtual void set_local_map(sensor_msgs::PointCloud2ConstPtr &local_map_ptr);
|
|
virtual void set_local_map_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr &pcl_ptr);
|
|
virtual int compute_force(Eigen::Vector3d &goal, Eigen::Vector3d &desired_vel);
|
|
virtual void init(ros::NodeHandle &nh);
|
|
APF() {}
|
|
~APF() {}
|
|
|
|
typedef shared_ptr<APF> Ptr;
|
|
};
|
|
|
|
|
|
|
|
#endif
|