Skip to content

Commit

Permalink
Local Planner ROS1-Pending service/topics and test it
Browse files Browse the repository at this point in the history
  • Loading branch information
@jcobano
jcobano committed Aug 14, 2022
1 parent 1f8e1ec commit 1067e6b
Show file tree
Hide file tree
Showing 3 changed files with 250 additions and 1,067 deletions.
12 changes: 6 additions & 6 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,19 +184,19 @@ rosidl_target_interfaces(planner_ros_node
${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(planner_ros_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

add_executable(local_planner_node src/ROS/local_planner_node.cpp src/Planners/LocalPlanner.cpp include/Planners/LocalPlanner.hpp)
ament_target_dependencies(local_planner_node ${PROJECT_DEPENDENCIES})
rosidl_target_interfaces(local_planner_node
${PROJECT_NAME} "rosidl_typesupport_cpp")
target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)
#add_executable(local_planner_node src/ROS/local_planner_node.cpp src/Planners/LocalPlanner.cpp include/Planners/LocalPlanner.hpp)
#ament_target_dependencies(local_planner_node ${PROJECT_DEPENDENCIES})
#rosidl_target_interfaces(local_planner_node
# ${PROJECT_NAME} "rosidl_typesupport_cpp")
#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

#rosidl_target_interfaces(local_planner_node
# ${PROJECT_NAME} "rosidl_typesupport_cpp")
#target_link_libraries(local_planner_node ${${PROJECT_NAME}_LIBRARIES} -lstdc++fs Eigen3::Eigen -lcrypto -lssl)

install(TARGETS
planner_ros_node
local_planner_node
#local_planner_node
DESTINATION lib/${PROJECT_NAME})

#############
Expand Down
114 changes: 21 additions & 93 deletions include/Planners/LocalPlanner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@

#include <geometry_msgs/msg/pose_stamped.hpp>

// #include <theta_star/ThetaStar2D.hpp>
// #include <theta_star/ThetaStar3D.hpp>
// JAC: Add the algorithms needed
#include "Planners/AStar.hpp"
#include "Planners/AStarM2.hpp"
Expand Down Expand Up @@ -118,26 +116,9 @@ class LocalPlanner : public rclcpp::Node {

void plan();

//Calbacks and publication functions
void localCostMapCb(const nav_msgs::msg::OccupancyGrid::ConstRawPtr &lcp);
void localCostMapCb(const nav_msgs::msg::OccupancyGrid::SharedPtr lcp);

//Global Input trajectory
void globalTrjCb(const trajectory_msgs::msg::MultiDOFJointTrajectory::ConstRawPtr &traj);

//Used to know when to stop calculating local trajectories
void goalReachedCb(const std_msgs::msg::Bool::ConstRawPtr &data);

// void dynRecCb(theta_star_2d::LocalPlannerConfig &config, uint32_t level);

// TODO REVIEW THIS PART
bool stopPlanningSrvCb(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &rep);
bool pausePlanningSrvCb(std_srvs::srv::Trigger::Request &req, std_srvs::srv::Trigger::Response &rep);

bool arrivedToGoalSrvCb(std_srvs::srv::Empty::Request &req, std_srvs::srv::Empty::Response &resp);
void executePathGoalServerCB();
void executePathPreemptCB();
void dist2GoalCb(const std_msgs::msg::Float32::ConstRawPtr &dist);
// JAC: Removed when topics and/or services are confirmed about the reception of the global goal.
// void executePathGoalServerCB();
// void executePathPreemptCB();

private:
void resetFlags();
Expand All @@ -146,97 +127,62 @@ class LocalPlanner : public rclcpp::Node {

void configTopics();
//Config thetastar class parameters
void configTheta();
void configServices();

void publishExecutePathFeedback();

// JAC: Decide if this is considered from AS2.
// void publishExecutePathFeedback();
geometry_msgs::msg::TransformStamped getTfMapToRobot();

// void configParams2D(); //JAC: IT IS NOT NEEDED
// void publishTrajMarker2D(); //JAC: IT IS NOT NEEDED
// void buildAndPubTrayectory2D(); //JAC: IT IS NOT NEEDED
// bool calculateLocalGoal2D(); //JAC: IT IS NOT NEEDED
// void calculatePath2D(); //JAC: IT IS NOT NEEDED
void buildAndPubTrayectory3D();
bool transformTrajectoryToFrame(std::string dest_frame);
void publishTrajMarker3D();
void configParams3D(); // JAC: Delete if it is not needed.
// void publishTrajMarker3D();
void configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic);
bool calculateLocalGoal3D();
void calculatePath3D();

//Auxiliar functions
void showTime(std::string message, struct timeb st, struct timeb ft);
bool pointInside(geometry_msgs::msg::Vector3 p);
geometry_msgs::msg::Point makePoint(const geometry_msgs::msg::Vector3 &vec);

//Add the occupied borders around received costmap
void inflateCostMap();
//Set to 0 the positions near the local goal in the border
void freeLocalGoal();
// void collisionMapCallBack(const octomap_msgs::OctomapConstPtr &msg); //JAC: IT IS NOT NEEDED
void pointsSub(const sensor_msgs::msg::PointCloud2::RawPtr &msg);
void configMarkers(std::string ns, std::string frame);

inline double euclideanDistance(double x0, double y0, double x, double y)
{
return sqrt(pow(x - x0, 2) + pow(y - y0, 2));
}
inline double euclideanDistance(double x0, double y0, double z0, double x, double y, double z)
{
return sqrt(pow(x - x0, 2) + pow(y - y0, 2) + pow(z - z0, 2));
}
//Variables
// ros::NodeHandlePtr nh;
// ros::ServiceClient costmap_clean_srv;
rclcpp::Service<std_srvs::srv::Trigger> costmap_clean_srv;
// rclcpp::Service<std_srvs::srv::Trigger> costmap_clean_srv;

// ros::Subscriber local_map_sub;
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr local_map_sub_;
// rclcpp::Subscription local_map_sub_;

// ros::Publisher visMarkersPublisher, trajPub, costmap_inflated_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr visMarkersPublisher, trajPub, costmap_inflated_pub_;
// JAC: trajPub with the corresponding message
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr visMarkersPublisher, trajPub;
// rclcpp::Publisher visMarkersPublisher, trajPub, costmap_inflated_pub_;

//Flags publishers

bool showConfig;
bool mapGeometryConfigured;
//Flow control flags
//To calculate planning time
struct timeb startT, finishT;
// struct timeb startT, finishT;

//Theta star algorithm parameters
//Geometric params
//Algorithm params
float cost_weight;
float lof_distance;
float occ_threshold;
//Traj params
//Costmap modificaton params
float localCostMapInflationX;
float localCostMapInflationY;
float border_space;
float occ_threshold;

double initialSearchAround, finalSearchAround;
//

unsigned int startIter;
int impossibleCnt, occGoalCnt, timesCleaned, badGoal;
int occGoalCnt, badGoal;

int number_of_points;
bool debug;
float seconds, milliseconds;

std::string robot_base_frame, world_frame,traj_dest_frame;
nav_msgs::msg::OccupancyGrid localCostMap, localCostMapInflated;
trajectory_msgs::msg::MultiDOFJointTrajectory globalTrajectory, localTrajectory;

// JAC: Change the type of messafe
// trajectory_msgs::msg::MultiDOFJointTrajectory globalTrajectory, localTrajectory;
//Markers
visualization_msgs::msg::Marker lineMarker, waypointsMarker;

geometry_msgs::msg::Vector3 local_costmap_center, localGoal, robotPose;
// ThetaStar2D theta2D;
geometry_msgs::msg::Vector3 localGoal, robotPose;
tf2_ros::Buffer *tfBuffer;

std_msgs::msg::Bool is_running;
Expand Down Expand Up @@ -272,32 +218,14 @@ class LocalPlanner : public rclcpp::Node {
double ws_y_min;
double ws_z_min;
double map_resolution;
double map_h_inflaction;
double map_v_inflaction; //JAC: Hasta aquí todo cero.
double goal_weight;
double z_weight_cost;
double z_not_inflate;
double traj_dxy_max;
double traj_dz_max;
double traj_vxy_m;
double traj_vz_m;
double traj_vxy_m_1;
double traj_vz_m_1;
double traj_wyaw_m;
double traj_pos_tol;
double traj_yaw_tol;
bool data_source;

double arrivedThresh;
octomap_msgs::msg::Octomap::ConstRawPtr map_;
// FIXME :: WHAT IS THIS
// ThetaStar3D theta3D;
double inflate_map;

bool use3d;
bool mapReceived;

trajectory_msgs::msg::MultiDOFJointTrajectoryPoint currentGoal;
std::vector<trajectory_msgs::msg::MultiDOFJointTrajectoryPoint> goals_vector,goals_vector_bl_frame;
// JAC: Change to the right message of the goal. TODO
// trajectory_msgs::msg::MultiDOFJointTrajectoryPoint currentGoal; TODO
// std::vector<trajectory_msgs::msg::MultiDOFJointTrajectoryPoint> goals_vector,goals_vector_bl_frame;
int last;
// std::unique_ptr<tf2_ros::TransformListener> tf_list_ptr;
double timeout;
Expand Down
Loading

0 comments on commit 1067e6b

Please sign in to comment.