From 221081d2b543fbd0a0ccb8e5f265f2a4aef57f34 Mon Sep 17 00:00:00 2001 From: jcobano Date: Thu, 19 Jan 2023 15:10:13 +0100 Subject: [PATCH] computation of LocalGrid with kdtree --- include/Grid3D/local_grid3d.hpp | 487 ++++++++++++++++++++ launch/frames.launch | 39 ++ launch/local_planner.launch | 84 ++++ rviz/local_planner.rviz | 391 ++++++++++++++++ src/ROS/local_planner_ros_node.cpp | 706 +++++++++++++++++++++++++++++ 5 files changed, 1707 insertions(+) create mode 100644 include/Grid3D/local_grid3d.hpp create mode 100644 launch/frames.launch create mode 100644 launch/local_planner.launch create mode 100644 rviz/local_planner.rviz create mode 100644 src/ROS/local_planner_ros_node.cpp diff --git a/include/Grid3D/local_grid3d.hpp b/include/Grid3D/local_grid3d.hpp new file mode 100644 index 0000000..955629c --- /dev/null +++ b/include/Grid3D/local_grid3d.hpp @@ -0,0 +1,487 @@ + #ifndef __LOCAL_GRID3D_HPP__ +#define __LOCAL_GRID3D_HPP__ + +/** + * @file prob_map.cpp + * @brief This file includes the ROS node implementation. + * @author Francisco J. Perez Grau, Fernando Caballero and José Antonio Cobano + */ + +#include +#include +#include +#include +#include +#include +#include +// PCL +#include +#include +#include +#include + +#include // for PCL_EXPORTS +#include // for pcl::isFinite +#include +#include +#include + +#include "utils/utils.hpp" + +#include +#include //for std::ostringstream +#include +#include //for std::setw, std::hex, and std::setfill +#include //for all other OpenSSL function calls +#include //for SHA512_DIGEST_LENGTH +// #include "utils/ros/ROSInterfaces.hpp" + +// #ifdef BUILD_VORONOI +// #include "voro++-0.4.6/src/voro++.hh" +// #endif + +class Local_Grid3d +{ +private: + + // Ros parameters + ros::NodeHandle m_nh; + bool m_publishPc; + std::string m_mapPath, m_nodeName; + std::string m_globalFrameId; + float m_sensorDev, m_gridSlice; + double m_publishPointCloudRate, m_publishGridSliceRate; + + // Octomap parameters + float m_maxX, m_maxY, m_maxZ; + float m_resolution, m_oneDivRes; + octomap::OcTree *m_octomap; + + // 3D probabilistic grid cell + Planners::utils::gridCell *m_grid; + int m_gridSize, m_gridSizeX, m_gridSizeY, m_gridSizeZ; + int m_gridStepY, m_gridStepZ; + + // 3D point cloud representation of the map + pcl::PointCloud::Ptr m_cloud, filter_cloud; + pcl::KdTreeFLANN m_kdtree; + + // Visualization of the map as pointcloud + sensor_msgs::PointCloud2 m_pcMsg; + ros::Publisher m_pcPub, percent_computed_pub_; + ros::Timer mapTimer; + + // Visualization of a grid slice as 2D grid map msg + nav_msgs::OccupancyGrid m_gridSliceMsg; + ros::Publisher m_gridSlicePub; + ros::Timer gridTimer; + + //Parameters added to allow a new exp function to test different gridmaps + double cost_scaling_factor, robot_radius; + bool use_costmap_function; + +public: + // Local_Grid3d(): m_cloud(new pcl::PointCloud) + Local_Grid3d(): m_cloud(new pcl::PointCloud) + { + // Load parameters INCLUDE PARAMETERS OF THE SIZE OF THE LOCAL SIZE + double value; + ros::NodeHandle lnh("~"); + lnh.param("name", m_nodeName, std::string("local_grid3d")); + if(!lnh.getParam("global_frame_id", m_globalFrameId)) // JAC: Local planner --> base_link or occupancy map? + m_globalFrameId = "map"; + // if(!lnh.getParam("map_path", m_mapPath)) + // m_mapPath = "map.ot"; + if(!lnh.getParam("publish_point_cloud", m_publishPc)) + m_publishPc = true; + if(!lnh.getParam("publish_point_cloud_rate", m_publishPointCloudRate)) + m_publishPointCloudRate = 0.2; + if(!lnh.getParam("publish_grid_slice", value)) + value = 1.0; + if(!lnh.getParam("publish_grid_slice_rate", m_publishGridSliceRate)) + m_publishGridSliceRate = 0.2; + m_gridSlice = (float)value; + if(!lnh.getParam("sensor_dev", value)) + value = 0.2; + m_sensorDev = (float)value; + + lnh.param("cost_scaling_factor", cost_scaling_factor, 0.8); //0.8 + lnh.param("robot_radius", robot_radius, 0.4); //0.4 + lnh.param("use_costmap_function", use_costmap_function, (bool)true); + + m_grid = NULL; + if(m_grid != NULL) + delete []m_grid; + + // configureParameters(); + if(configureParameters()) + { + computeLocalGrid(m_cloud); + + // Build the msg with a slice of the grid if needed + if(m_gridSlice >= 0 && m_gridSlice <= m_maxZ) + { + buildGridSliceMsg(m_gridSlice); + m_gridSlicePub = m_nh.advertise(m_nodeName+"/grid_slice", 1, true); + gridTimer = m_nh.createTimer(ros::Duration(1.0/m_publishGridSliceRate), &Local_Grid3d::publishGridSliceTimer, this); + } + + // Setup point-cloud publisher + if(m_publishPc) + { + m_pcPub = m_nh.advertise(m_nodeName+"/map_point_cloud", 1, true); + mapTimer = m_nh.createTimer(ros::Duration(1.0/m_publishPointCloudRate), &Local_Grid3d::publishMapPointCloudTimer, this); + } + percent_computed_pub_ = m_nh.advertise(m_nodeName+"/percent_computed", 1, false); + } + else{ + // std::cout << "\tCOMPUTE GRID" << std::endl; + // computeLocalGrid(m_cloud); + // JAC: Here "Build the msg with a slice of the grid if needed" should be? + } + + } + + ~Local_Grid3d(void) + { + // if(m_octomap != NULL) + // delete m_octomap; + if(m_grid != NULL) + delete []m_grid; + } + bool setCostParams(const double &_cost_scaling_factor, const double &_robot_radius){ + + cost_scaling_factor = std::fabs(_cost_scaling_factor); + robot_radius = std::fabs(_robot_radius); + + return true; + } + + void setGridSliceHeight(const double _height){ + if(_height < m_maxZ && _height > 0.0 ){ + m_gridSlice = _height; + buildGridSliceMsg(m_gridSlice); + } + } + + void publishMapPointCloud(void) + { + m_pcMsg.header.stamp = ros::Time::now(); + m_pcPub.publish(m_pcMsg); + } + + void publishGridSlice(void) + { + m_gridSliceMsg.header.stamp = ros::Time::now(); + m_gridSlicePub.publish(m_gridSliceMsg); + } + + bool isIntoMap(float x, float y, float z) + { + return (x >= 0.0 && y >= 0.0 && z >= 0.0 && x < m_maxX && y < m_maxY && z < m_maxZ); + } + // int getCellCost(const float &_x, const float &_y, const float &_z){ + // JAC Precision + float getCellCost(const float &_x, const float &_y, const float &_z){ + + if( !isIntoMap(_x, _y, _z) ){ + std::cout << "OUT " << std::endl; + return 0; + } + + + int index = point2grid(_x, _y, _z); + + // return m_grid[index].prob; + return m_grid[index].dist; + } + + // JAC + void computeLocalGrid(const pcl::PointCloud::ConstPtr &_cloud) //void + { + // unsigned t0, t1, t2, t3, t4, t5; + + //Publish percent variable + // std_msgs::Float32 percent_msg; + // percent_msg.data = 0; + + // Alloc the 3D grid JAC: Alloc the size of the local grid + m_gridSizeX = (int)(m_maxX*m_oneDivRes); + m_gridSizeY = (int)(m_maxY*m_oneDivRes); + m_gridSizeZ = (int)(m_maxZ*m_oneDivRes); + m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + m_gridStepY = m_gridSizeX; + m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // JAC: Pongo los mismo datos que en global planner y no funciona + // m_gridSizeX = 341; + // m_gridSizeY = 241; + // m_gridSizeZ = 101; + // m_gridSize = m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // m_gridStepY = m_gridSizeX; + // m_gridStepZ = m_gridSizeX*m_gridSizeY; + + // JAC: Alloc the 3D local Grid from input parameters. These parameters should be defined once at the beginning. + // release previously loaded data + if(m_grid != NULL) + delete []m_grid; + // t0 = clock(); + m_grid = new Planners::utils::gridCell[m_gridSize]; // 2-3 milliseconds + // t1 = clock(); + // double time = (double(t1-t0)/CLOCKS_PER_SEC); + // std::cout << "Execution Time: " << time << std::endl; + + // std::cout << "_maxX: " << m_maxX << std::endl; + // std::cout << "_gridSizeX: " << m_gridSizeX << std::endl; + + // Setup kdtree + // std::vector ind; + // pcl::removeNaNFromPointCloud(*_cloud, *filter_cloud, ind); // JAC: Falla aqui + // std::cout << "no. of pts in input=" << _cloud->size() << std::endl; //JAC: Confirmado que el pointcloud se pasa bien + // std::cout << "no. of pts in output="<< filter_cloud->size() << std::endl; + // t2 = clock(); + m_kdtree.setInputCloud(_cloud); //Less than 1 millisecond + // t3 = clock(); + // double time2 = (double(t3-t2)/CLOCKS_PER_SEC); + // std::cout << "Execution Time2: " << time2 << std::endl; + + // Compute the distance to the closest point of the grid + int index; + float dist; + // float gaussConst1 = 1./(m_sensorDev*sqrt(2*M_PI)); + // float gaussConst2 = 1./(2*m_sensorDev*m_sensorDev); + // pcl::PointXYZI searchPoint; + pcl::PointXYZ searchPoint; + std::vector pointIdxNKNSearch(1); + std::vector pointNKNSquaredDistance(1); + double count=0; + // double percent; + // double size=m_gridSizeX*m_gridSizeY*m_gridSizeZ; + // t4 = clock(); + // JAC: 2-3 seconds + for(int iz=0; iz percent_msg.data + 0.5){ + // percent_msg.data = percent; + // // percent_computed_pub_.publish(percent_msg); + // } + + // JAC Error:Define correctly size of local map + // JAC: AQUI PETA CUANDO NO SE RECIBE POINTCLOUD + if(m_kdtree.nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) + { + dist = pointNKNSquaredDistance[0]; + m_grid[index].dist = dist; // dist in the discrete world + // if(!use_costmap_function){ + // m_grid[index].prob = gaussConst1*exp(-dist*dist*gaussConst2); + // }else{ + // double prob = 100*exp(-cost_scaling_factor*std::fabs((dist - robot_radius))); + // // ROS_INFO("[%f, %f, %f] Dist: %f Probability: %f", searchPoint.x, searchPoint.y, searchPoint.z, dist, prob); + // //JAC: Include the computation of prob considering the distance to the nearest voronoi edge. + // m_grid[index].prob = prob; + // } + } + else + { + m_grid[index].dist = -1.0; + // m_grid[index].prob = 0.0; + } + + } + } + } + // t5 = clock(); + // double time3 = (double(t5-t4)/CLOCKS_PER_SEC); + // std::cout << "Execution Time3: " << time3 << std::endl; + + // percent_msg.data = 100; + // percent_computed_pub_.publish(percent_msg); + } + + std::pair getClosestObstacle(const Planners::utils::Vec3i& _coords){ + + // pcl::PointXYZI searchPoint; + pcl::PointXYZ searchPoint; + + searchPoint.x = _coords.x * m_resolution; + searchPoint.y = _coords.y * m_resolution; + searchPoint.z = _coords.z * m_resolution; + + int k = 1; + m_kdtree.setInputCloud(m_cloud); + std::vector pointIdxNKNSearch(k); + std::vector pointNKNSquaredDistance(k); + + if(m_kdtree.nearestKSearch(searchPoint, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0){ + + Planners::utils::Vec3i result; + + result.x = std::round(m_cloud->points[pointIdxNKNSearch[0]].x / m_resolution); + result.y = std::round(m_cloud->points[pointIdxNKNSearch[0]].y / m_resolution); + result.z = std::round(m_cloud->points[pointIdxNKNSearch[0]].z / m_resolution); + + return std::make_pair(result, std::sqrt(pointNKNSquaredDistance[k-1])); + }else{ + return std::make_pair(Planners::utils::Vec3i{}, std::numeric_limits::max()); + } + } + +protected: + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" + void publishMapPointCloudTimer(const ros::TimerEvent& event) + { + publishMapPointCloud(); + } + + void publishGridSliceTimer(const ros::TimerEvent& event) + { + publishGridSlice(); + } +#pragma GCC diagnostic pop + + // JAC: Configure parameters of the size of the Local Grid + bool configureParameters(void) + { + if(m_grid != NULL) + delete []m_grid; + + // Get map parameters: They have to take from local_world_size_x, local_world_size_y , local_world_size_z of the launch + double minX, minY, minZ, maxX, maxY, maxZ, res; + + maxX = 10.0; + minX = 0.0; + maxY = 10.0; + minY = 0.0; + maxZ = 4.0; + minZ = 0.0; + res = 0.05; + + // maxX = 4.0; + // minX = 0.0; + // maxY = 4.0; + // minY = 0.0; + // maxZ = 2.0; + // minZ = 0.0; + // res = 0.1; + + m_maxX = (float)(maxX-minX); + m_maxY = (float)(maxY-minY); + m_maxZ = (float)(maxZ-minZ); + m_resolution = (float)res; + m_oneDivRes = 1.0/m_resolution; + + + // float workspace_x, workspace_y, workspace_z; + + // m_nh.param("local_world_size_x", workspace_x, (float)2.0); // In meters + // m_nh.param("local_world_size_y", workspace_y, (float)2.0); // In meters + // m_nh.param("local_world_size_z", workspace_z, (float)1.0); // In meters + // m_nh.param("resolution", m_resolution, (float)0.1); + + // maxX = (float)(2*workspace_x); + // minX = (float)(0.0); + // maxY = (float)(2*workspace_y); + // minY = (float)(0.0); + // maxZ = (float)(2*workspace_z); + // minZ = (float)(0.0); + + // std::cout << "workspace_x: " << workspace_x << std::endl; + // std::cout << "maxX: " << maxX << std::endl; + + // m_maxX = (float)(maxX-minX); + // m_maxY = (float)(maxY-minY); + // m_maxZ = (float)(maxZ-minZ); + // // m_resolution = (float)res; + // m_oneDivRes = 1.0/m_resolution; + + return false; + } + + std::string bytes_to_hex_string(const std::vector& bytes) + { + std::ostringstream stream; + for (uint8_t b : bytes) + { + stream << std::setw(2) << std::setfill('0') << std::hex << static_cast(b); + } + return stream.str(); + } + //perform the SHA3-512 hash + std::string sha3_512(const char * _input,int _size) + { + uint32_t digest_length = SHA512_DIGEST_LENGTH; + const EVP_MD* algorithm = EVP_sha3_512(); + uint8_t* digest = static_cast(OPENSSL_malloc(digest_length)); + EVP_MD_CTX* context = EVP_MD_CTX_new(); + EVP_DigestInit_ex(context, algorithm, nullptr); + EVP_DigestUpdate(context, _input, _size); + EVP_DigestFinal_ex(context, digest, &digest_length); + EVP_MD_CTX_destroy(context); + std::string output = bytes_to_hex_string(std::vector(digest, digest + digest_length)); + OPENSSL_free(digest); + return output; + } + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-result" +#pragma GCC diagnostic pop + + void buildGridSliceMsg(float z) + { + static int seq = 0; + + // Setup grid msg + m_gridSliceMsg.header.frame_id = m_globalFrameId; + m_gridSliceMsg.header.stamp = ros::Time::now(); + m_gridSliceMsg.header.seq = seq++; + m_gridSliceMsg.info.map_load_time = ros::Time::now(); + m_gridSliceMsg.info.resolution = m_resolution; + m_gridSliceMsg.info.width = m_gridSizeX; + m_gridSliceMsg.info.height = m_gridSizeY; + m_gridSliceMsg.info.origin.position.x = 0.0; + m_gridSliceMsg.info.origin.position.y = 0.0; + m_gridSliceMsg.info.origin.position.z = z; + m_gridSliceMsg.info.origin.orientation.x = 0.0; + m_gridSliceMsg.info.origin.orientation.y = 0.0; + m_gridSliceMsg.info.origin.orientation.z = 0.0; + m_gridSliceMsg.info.origin.orientation.w = 1.0; + m_gridSliceMsg.data.resize(static_cast(m_gridSizeX)*m_gridSizeY); + + // Extract max probability + int offset = (int)(z*m_oneDivRes)*m_gridSizeX*m_gridSizeY; + int end = offset + m_gridSizeX*m_gridSizeY; + float maxProb = -1.0; + for(int i=offset; i maxProb) + maxProb = m_grid[i].prob; + + // Copy data into grid msg and scale the probability to [0,100] + if(maxProb < 0.000001) + maxProb = 0.000001; + maxProb = 100.0/maxProb; + for(int i=0; i + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/local_planner.launch b/launch/local_planner.launch new file mode 100644 index 0000000..d736cc5 --- /dev/null +++ b/launch/local_planner.launch @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rviz/local_planner.rviz b/rviz/local_planner.rviz new file mode 100644 index 0000000..f4581e7 --- /dev/null +++ b/rviz/local_planner.rviz @@ -0,0 +1,391 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.47089946269989014 + Tree Height: 1522 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: PointCloud2_Velodyne +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: false + base_link: + Value: true + base_stabilized: + Value: false + laser0: + Value: false + laser0_base_link: + Value: false + map: + Value: true + occupancy_map: + Value: true + world: + Value: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + occupancy_map: + {} + world: + base_footprint: + base_stabilized: + base_link: + laser0_base_link: + laser0: + {} + Update Interval: 0 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/path_line_markers + Name: Path Line Markers + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/astar_explored_nodes + Name: Explored Nodes + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/path_points_markers + Name: Path Point Markers + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/aux_text_marker + Name: Aux Text Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/best_node_marker + Name: Best Node Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/closed_set_nodes + Name: Closed Set Nodes + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /planner_ros_node/openset_nodes + Name: Open Set Nodes + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: false + Name: GridSlice + Topic: /grid3d/grid_slice + Unreliable: false + Use Timestamp: false + Value: false + - Alpha: 0.699999988079071 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 19.80000114440918 + Min Value: 0 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Occupation Markers + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.20000000298023224 + Style: Flat Squares + Topic: /planner_ros_node/occupancy_markers + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 20.100000381469727 + Min Value: 0.10000000149011612 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Map PointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /grid3d/map_point_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Marker + Enabled: true + Marker Topic: /test_text_marker + Name: Test Text Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: -1.1940319538116455 + Min Value: -3.9992451667785645 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Local Map Occupation Markers + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /local_planner_ros_node/local_occupancy_markers + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /ground_truth_to_tf/pose + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2_Velodyne + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /velodyne_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.2906503677368164 + Min Value: -1.1320875883102417 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2_LocalMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /cloud_PCL + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 24.63131332397461 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 1.5664501190185547 + Y: 4.230220317840576 + Z: -14.341166496276855 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.339796543121338 + Target Frame: + Yaw: 4.657133102416992 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002f6000006eafc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006ea0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e700000005efc0100000002fb0000000800540069006d0065010000000000000e70000006dc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000b6e000006ea00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3696 + X: 2704 + Y: 54 diff --git a/src/ROS/local_planner_ros_node.cpp b/src/ROS/local_planner_ros_node.cpp new file mode 100644 index 0000000..cb34f60 --- /dev/null +++ b/src/ROS/local_planner_ros_node.cpp @@ -0,0 +1,706 @@ +#include + +#include "Planners/AStar.hpp" +#include "Planners/AStarM2.hpp" +#include "Planners/AStarM1.hpp" +#include "Planners/ThetaStar.hpp" +#include "Planners/ThetaStarM1.hpp" +#include "Planners/ThetaStarM2.hpp" +#include "Planners/LazyThetaStar.hpp" +#include "Planners/LazyThetaStarM1.hpp" +#include "Planners/LazyThetaStarM1Mod.hpp" +#include "Planners/LazyThetaStarM2.hpp" +#include "utils/ros/ROSInterfaces.hpp" +#include "utils/SaveDataVariantToFile.hpp" +#include "utils/misc.hpp" +#include "utils/geometry_utils.hpp" +#include "utils/metrics.hpp" + +#include "Grid3D/local_grid3d.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + + +/** + * @brief Demo Class that demonstrate how to use the algorithms classes and utils + * with ROS + * + */ +class HeuristicLocalPlannerROS +{ + +public: + HeuristicLocalPlannerROS() + { + + std::string algorithm_name; + lnh_.param("algorithm", algorithm_name, (std::string)"astar"); + lnh_.param("heuristic", heuristic_, (std::string)"euclidean"); + + configureAlgorithm(algorithm_name, heuristic_); + + // pointcloud_local_sub_ = lnh_.subscribe>("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); + // pointcloud_local_sub_ = lnh_.subscribe("/points", 1, &HeuristicLocalPlannerROS::pointCloudCallback, this); //compile + occupancy_grid_local_sub_ = lnh_.subscribe("/grid", 1, &HeuristicLocalPlannerROS::occupancyGridCallback, this); + + // request_path_server_ = lnh_.advertiseService("request_path", &HeuristicLocalPlannerROS::requestPathService, this); // This is in planner_ros_node.cpp and the corresponding service defined. + change_planner_server_ = lnh_.advertiseService("set_algorithm", &HeuristicLocalPlannerROS::setAlgorithm, this); + + line_markers_pub_ = lnh_.advertise("path_line_markers", 1); + point_markers_pub_ = lnh_.advertise("path_points_markers", 1); + cloud_test = lnh_.advertise >("/cloud_PCL", 1, true); // Defined by me to show the point cloud as pcl::PointCloudisActive()) + // { + // clearMarkers(); + // return; + // } + + calculatePath3D(); + // state.reset(new actionlib::SimpleClientGoalState(navigation3DClient->getState())); + + // seconds = finishT.time - startT.time - 1; + // milliseconds = (1000 - startT.millitm) + finishT.millitm; + // publishExecutePathFeedback(); + + // if (*state == actionlib::SimpleClientGoalState::SUCCEEDED) + // { + // clearMarkers(); + // action_result.arrived = true; + // execute_path_srv_ptr->setSucceeded(action_result); + // ROS_ERROR("LocalPlanner: Goal Succed"); + // return; + // } + // else if (*state == actionlib::SimpleClientGoalState::ABORTED) + // { + // ROS_INFO_COND(debug, "Goal aborted by path tracker"); + // resetFlags(); + // } + // else if (*state == actionlib::SimpleClientGoalState::PREEMPTED) + // { + // ROS_INFO_COND(debug, "Goal preempted by path tracker"); + // resetFlags(); + // } + //ROS_INFO_COND(debug, "Before start loop calculation"); + } + + +private: + + void occupancyGridCallback(const nav_msgs::OccupancyGrid::ConstPtr &_grid){ + ROS_INFO("Loading OccupancyGrid map..."); + Planners::utils::configureWorldFromOccupancyWithCosts(*_grid, *algorithm_); + algorithm_->publishOccupationMarkersMap(); + occupancy_grid_local_sub_.shutdown(); + ROS_INFO("Occupancy Grid Loaded"); + occupancy_grid_ = *_grid; + input_map_ = 1; + } + + // From lazy_theta_star_planners + // void pointCloudCallback(const PointCloud::ConstPtr &points) + // void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &_points) + void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud) + { + // // ROS_INFO_COND(debug, PRINTF_MAGENTA "Collision Map Received"); + mapReceived = true; // At the end to run the calculate3Dpath after generating PointCloud? + sensor_msgs::PointCloud2 base_cloud; + + // laser0 in bag + // std::cout << "frame cloud: " << cloud->header.frame_id << std::endl; + + // Pre-cache transform for point-cloud to base frame and transform the pc + if(!m_tfCache) + { + try + { + // Providing ros::Time(0) will just get us the latest available transform. + // ros::Duration(2.0) --> Duration before timeout + + m_tfListener.waitForTransform(baseFrameId, cloud->header.frame_id, ros::Time(0), ros::Duration(2.0)); + // Get the transform between two frames by frame ID. + // lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const + m_tfListener.lookupTransform(baseFrameId, cloud->header.frame_id, ros::Time(0), pclTf); + m_tfCache = true; + } + catch (tf::TransformException &ex) + { + ROS_ERROR("%s",ex.what()); + return; + } + } + + // base_cloud.header.frame_id is laser0 (like cloud) + // base_cloud=*cloud; + // base_cloud.header.frame_id = + + // transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out) + + // Transform a sensor_msgs::PointCloud2 dataset from its frame to a given TF target frame. + pcl_ros::transformPointCloud(baseFrameId, pclTf, *cloud, base_cloud); // Transform pointcloud to our TF --> baseCloud + // pcl_ros::transformPointCloud(globalFrameId, pclTf, *cloud, base_cloud); // Transform pointcloud to map/world (globalFrameId) --> baseCloud JAC + + // std::cout << "frame base_cloud: " << base_cloud.header.frame_id << std::endl; + // std::cout << "Height: " << base_cloud.height << std::endl; + // std::cout << "Width: " << base_cloud.width << std::endl; //width is the length of the point cloud + // std::cout << "Size: " << base_cloud.size() << std::endl; + // ROS_INFO("PointCloud received"); + + // PointCloud2 to PointXYZ conversion (base_cloud --> downCloud), with range limits [0,5000]: 5000 is defined in PointCloud2_to_PointXYZ + // std::vector downCloud; + // PointCloud2_to_PointXYZ(base_cloud, downCloud); + + // JAC: Decide if convert std::vector to pcl::PointCloud after obtaining downCloud or use PointCloud2_to_PointXYZ_pcl. Now, the latter because in ROSInterfaces.cpp is used. + + + // Convert std::vector to pcl::PointCloud + // std::vector downCloud; + // pcl::PointCloud local_cloud_; + // for(unsigned int i=0; i local_cloud_; + local_cloud_.header.frame_id = baseFrameId; + + // sensor_msgs::PointCloud2 --> pcl::PointCloud to configure the local map. Only points inside local map are considered. + PointCloud2_to_PointXYZ_pcl(base_cloud,local_cloud_); + // std::cout << "Size: " << local_cloud_.size() << std::endl; + // std::cout << "frame local_cloud: " << local_cloud_.header.frame_id << std::endl; + + // Publisher to show that local_cloud_ is correctly generated and it corresponds to the point cloud from velodyne. + cloud_test.publish(local_cloud_); + + // Clean the world before generating it for each iteration + algorithm_->cleanLocalWorld(); + // Configure the local world from the PointCloud (for the local map) + Planners::utils::configureWorldFromPointCloud(boost::make_shared>(local_cloud_), *algorithm_, resolution_); + // Publish the Occupancy Map + algorithm_->publishLocalOccupationMarkersMap(); + + // Configure the distance grid and the cost grid + Planners::utils::configureLocalWorldCosts(boost::make_shared>(local_cloud_), *m_local_grid3d_, *algorithm_); // JAC: Is this correctly generated? + + // ROS_INFO("Published occupation marker local map"); + + + // cloud_ = *_points; + // input_map_ = 2; + // pointcloud_local_sub_.shutdown(); + + // std::cout << "Size: " << downCloud.size() << std::endl; + // for(unsigned int i=0; i &out) + { + sensor_msgs::PointCloud2Iterator iterX(in, "x"); + sensor_msgs::PointCloud2Iterator iterY(in, "y"); + sensor_msgs::PointCloud2Iterator iterZ(in, "z"); + out.clear(); + for(unsigned int i=0; i=-(size_x) && p.y<=size_y && p.y>=-(size_y) && p.z<=size_z && p.z>=-(size_z)) + out.push_back(p); + } + + return true; + } + + bool PointCloud2_to_PointXYZ(sensor_msgs::PointCloud2 &in, std::vector &out) + { + sensor_msgs::PointCloud2Iterator iterX(in, "x"); + sensor_msgs::PointCloud2Iterator iterY(in, "y"); + sensor_msgs::PointCloud2Iterator iterZ(in, "z"); + out.clear(); + for(unsigned int i=0; i 1 && d2 < 5000) + out.push_back(p); + } + + return true; + } + + void calculatePath3D() +{ + if (mapReceived) // It is true once a pointcloud is received. + { + // ROS_INFO("Local Planner 3D: Global trj received and pointcloud received"); + mapReceived = false; + + // //TODO : Set to robot pose to the center of the workspace? + // if (theta3D.setValidInitialPosition(robotPose) || theta3D.searchInitialPosition3d(initialSearchAround)) + // { + // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Calculating local goal"); + + // if (calculateLocalGoal3D()) //TODO: VER CUAL SERIA EL LOCAL GOAL EN ESTE CASO + // { + // ROS_INFO_COND(debug, PRINTF_MAGENTA "Local Planner 3D: Local Goal calculated"); + + // // ROS_INFO("%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); + // RCLCPP_INFO(this->get_logger(),"%.6f,%.6f, %.6f", localGoal.x, localGoal.y,localGoal.z); + + // if (!theta3D.isInside(localGoal)) + // { + // ROS_INFO("Returning, not inside :("); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Not inside after second check"); + // navigation3DClient->cancelAllGoals(); + // return; + // } + + // if (theta3D.setValidFinalPosition(localGoal) || theta3D.searchFinalPosition3dAheadHorizontalPrior(finalSearchAround)) + // { + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Computing Local Path"); + + // // JAC: Should we calculate the path with findpath (which return the path instead of the number of points)? + // number_of_points = theta3D.computePath(); + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Path computed, %d points", number_of_points); + // occGoalCnt = 0; + + // if (number_of_points > 0) + // { + // buildAndPubTrayectory3D(); + // planningStatus.data = "OK"; + // if (impossibleCnt > 0) //If previously the local planner couldn t find solution, reset + // impossibleCnt = 0; + // } + // else if (number_of_points == 0) //!Esto es lo que devuelve el algoritmo cuando NO HAY SOLUCION + // { + + // impossibleCnt++; + // //ROS_INFO_COND(debug,"Local: +1 impossible"); + // if (impossibleCnt > 2) + // { + + // clearMarkers(); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Requesting new global path, navigation cancelled"); + // navigation3DClient->cancelAllGoals(); + // planningStatus.data = "Requesting new global path, navigation cancelled"; + // impossibleCnt = 0; + // } + // } + // } + // else if (occGoalCnt > 2) //!Caso GOAL OCUPADO + // { //If it cant find a free position near local goal, it means that there is something there. + // ROS_INFO_COND(debug, PRINTF_BLUE "Local Planner 3D: Pausing planning, final position busy"); + // planningStatus.data = "Final position Busy, Cancelling goal"; + // //TODO What to tell to the path tracker + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result, "Local goal occupied"); + // //In order to resume planning, someone must call the pause/resume planning Service that will change the flag to true + // occGoalCnt = 0; + // } + // else + // { + // ++occGoalCnt; + // } + // } + // else if (badGoal < 3) + // { + // ROS_INFO_COND(debug, "Local Planner 3D: Bad Goal Calculated: [%.2f, %.2f]", localGoal.x, localGoal.y); + + // ++badGoal; + // } + // else + // { + // navigation3DClient->cancelAllGoals(); + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result,"Bad goal calculated 3 times"); + // badGoal = 0; + // ROS_INFO("Bad goal calculated 3 times"); + // } + // } + // else + // { + // planningStatus.data = "No initial position found..."; + // action_result.arrived=false; + // execute_path_srv_ptr->setAborted(action_result,"No initial position found"); + // navigation3DClient->cancelAllGoals(); + + // clearMarkers(); + // ROS_INFO_COND(debug, "Local Planner 3D: No initial free position found"); + // } + } + // else + // { + // ROS_INFO("Map not received"); + // } +} + + bool setAlgorithm(heuristic_planners::SetAlgorithmRequest &_req, heuristic_planners::SetAlgorithmResponse &rep){ + + configureAlgorithm(_req.algorithm.data, _req.heuristic.data); + rep.result.data = true; + return true; + } + + void configureAlgorithm(const std::string &algorithm_name, const std::string &_heuristic){ + + float ws_x, ws_y, ws_z; + + lnh_.param("local_world_size_x", ws_x, (float)2.0); // In meters + lnh_.param("local_world_size_y", ws_y, (float)2.0); // In meters + lnh_.param("local_world_size_z", ws_z, (float)1.0); // In meters + lnh_.param("resolution", resolution_, (float)0.1); + lnh_.param("inflate_map", inflate_, (bool)true); + + // world_size_.x = std::floor(ws_x / resolution_); + // world_size_.y = std::floor(ws_y / resolution_); + // world_size_.z = std::floor(ws_z / resolution_); + + local_world_size_meters.x=ws_x; + local_world_size_meters.y=ws_y; + local_world_size_meters.z=ws_z; + + local_world_size_.x = std::floor((2*ws_x) / resolution_); + local_world_size_.y = std::floor((2*ws_y) / resolution_); + local_world_size_.z = std::floor((2*ws_z) / resolution_); + + lnh_.param("use3d", use3d_, (bool)true); + + if( algorithm_name == "astar" ){ + ROS_INFO("Using A*"); + algorithm_.reset(new Planners::AStar(use3d_)); + }else if( algorithm_name == "costastar" ){ + ROS_INFO("Using Cost Aware A*"); + algorithm_.reset(new Planners::AStarM1(use3d_)); + }else if( algorithm_name == "astarsafetycost" ){ + ROS_INFO("Using A* Safety Cost"); + algorithm_.reset(new Planners::AStarM2(use3d_)); + }else if ( algorithm_name == "thetastar" ){ + ROS_INFO("Using Theta*"); + algorithm_.reset(new Planners::ThetaStar(use3d_)); + }else if ( algorithm_name == "costhetastar" ){ + ROS_INFO("Using Cost Aware Theta* "); + algorithm_.reset(new Planners::ThetaStarM1(use3d_)); + }else if ( algorithm_name == "thetastarsafetycost" ){ + ROS_INFO("Using Theta* Safety Cost"); + algorithm_.reset(new Planners::ThetaStarM2(use3d_)); + }else if( algorithm_name == "lazythetastar" ){ + ROS_INFO("Using LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStar(use3d_)); + }else if( algorithm_name == "costlazythetastar"){ + ROS_INFO("Using Cost Aware LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarM1(use3d_)); + }else if( algorithm_name == "costlazythetastarmodified"){ + ROS_INFO("Using Cost Aware LazyTheta*"); + algorithm_.reset(new Planners::LazyThetaStarM1Mod(use3d_)); + }else if( algorithm_name == "lazythetastarsafetycost"){ + ROS_INFO("Using LazyTheta* Safety Cost"); + algorithm_.reset(new Planners::LazyThetaStarM2(use3d_)); + }else{ + ROS_WARN("Wrong algorithm name parameter. Using ASTAR by default"); + algorithm_.reset(new Planners::AStar(use3d_)); + } + + // algorithm_->setWorldSize(local_world_size_, resolution_); + // JAC: QUITAR? + algorithm_->setLocalWorldSize(local_world_size_, resolution_); + + configureHeuristic(_heuristic); + + ROS_INFO("Using discrete local world size: [%d, %d, %d]", local_world_size_.x, local_world_size_.y, local_world_size_.z); + ROS_INFO("Using resolution: [%f]", resolution_); + + if(inflate_){ + double inflation_size; + lnh_.param("inflation_size", inflation_size, 0.05); + inflation_steps_ = std::round(inflation_size / resolution_); + ROS_INFO("Inflation size %.2f, using inflation step %d", inflation_size, inflation_steps_); + } + algorithm_->setInflationConfig(inflate_, inflation_steps_); + + m_local_grid3d_.reset(new Local_Grid3d); //TODO Costs not implement yet // Is this necessary in the Local Planner? + double cost_scaling_factor, robot_radius; + lnh_.param("cost_scaling_factor", cost_scaling_factor, 0.8); + lnh_.param("robot_radius", robot_radius, 0.4); + + m_local_grid3d_->setCostParams(cost_scaling_factor, robot_radius); + + // Read node parameters + // if(!lnh.getParam("in_cloud", m_inCloudTopic)) + // m_inCloudTopic = "/pointcloud"; + if(!lnh_.getParam("base_frame_id", baseFrameId)) + baseFrameId = "base_link"; + if(!lnh_.getParam("odom_frame_id", odomFrameId)) + odomFrameId = "odom"; + if(!lnh_.getParam("global_frame_id", globalFrameId)) + globalFrameId = "map"; + + // configMarkers(algorithm_name, globalFrameId, resolution_); // Not influence by baseFrameId + configMarkers(algorithm_name, baseFrameId, resolution_); + + // From planner_ros_node + // std::string frame_id; + // lnh_.param("frame_id", frame_id, std::string("map")); + // configMarkers(algorithm_name, frame_id, resolution_); + + lnh_.param("save_data_file", save_data_, (bool)true); + lnh_.param("data_folder", data_folder_, std::string("planing_data.txt")); + if(save_data_) + ROS_INFO_STREAM("Saving path planning data results to " << data_folder_); + + // JAC: This is not neccesary in local planner. + if( input_map_ == 1 ){ + Planners::utils::configureWorldFromOccupancyWithCosts(occupancy_grid_, *algorithm_); + }else if( input_map_ == 2 ){ + Planners::utils::configureWorldFromPointCloud(boost::make_shared>(cloud_), *algorithm_, resolution_); + // Planners::utils::configureLocalWorldCosts(*m_local_grid3d_, *algorithm_); //JAC: Discomment + ROS_INFO("CONFIGURED WORLD2"); + + } + //Algorithm specific parameters. Its important to set line of sight after configuring world size(it depends on the resolution) + float sight_dist, cost_weight; + lnh_.param("max_line_of_sight_distance", sight_dist, (float)1000.0); // In meters + lnh_.param("cost_weight", cost_weight, (float)0.0); + algorithm_->setMaxLineOfSight(sight_dist); + algorithm_->setCostFactor(cost_weight); + + lnh_.param("overlay_markers", overlay_markers_, (bool)false); + + // Init internal variables: TF transform + m_tfCache = false; + } + void configureHeuristic(const std::string &_heuristic){ + + if( _heuristic == "euclidean" ){ + algorithm_->setHeuristic(Planners::Heuristic::euclidean); + ROS_INFO("Using Euclidean Heuristics"); + }else if( _heuristic == "euclidean_optimized" ){ + algorithm_->setHeuristic(Planners::Heuristic::euclideanOptimized); + ROS_INFO("Using Optimized Euclidean Heuristics"); + }else if( _heuristic == "manhattan" ){ + algorithm_->setHeuristic(Planners::Heuristic::manhattan); + ROS_INFO("Using Manhattan Heuristics"); + }else if( _heuristic == "octogonal" ){ + algorithm_->setHeuristic(Planners::Heuristic::octagonal); + ROS_INFO("Using Octogonal Heuristics"); + }else if( _heuristic == "dijkstra" ){ + algorithm_->setHeuristic(Planners::Heuristic::dijkstra); + ROS_INFO("Using Dijkstra Heuristics"); + }else{ + algorithm_->setHeuristic(Planners::Heuristic::euclidean); + ROS_WARN("Wrong Heuristic param. Using Euclidean Heuristics by default"); + } + } + std::vector> getClosestObstaclesToPathPoints(const Planners::utils::CoordinateList &_path){ + + std::vector> result; + if ( use3d_ ){ + //TODO grid3d distances does not take into account the inflation added internally by the algorithm + + for(const auto &it: _path) + result.push_back( m_local_grid3d_->getClosestObstacle(it) ); + } + + else{//TODO IMplement for 2d + result.push_back(std::make_pair(Planners::utils::Vec3i{0,0,0}, 0.0)); + } + return result; + } + void configMarkers(const std::string &_ns, const std::string &_frame, const double &_scale){ + + path_line_markers_.ns = _ns; + path_line_markers_.header.frame_id = _frame; + path_line_markers_.header.stamp = ros::Time::now(); + path_line_markers_.id = rand(); + path_line_markers_.lifetime = ros::Duration(500); + path_line_markers_.type = visualization_msgs::Marker::LINE_STRIP; + path_line_markers_.action = visualization_msgs::Marker::ADD; + path_line_markers_.pose.orientation.w = 1; + + path_line_markers_.color.r = 0.0; + path_line_markers_.color.g = 1.0; + path_line_markers_.color.b = 0.0; + + path_line_markers_.color.a = 1.0; + path_line_markers_.scale.x = _scale; + + path_points_markers_.ns = _ns; + path_points_markers_.header.frame_id = _frame; + path_points_markers_.header.stamp = ros::Time::now(); + path_points_markers_.id = rand(); + path_points_markers_.lifetime = ros::Duration(500); + path_points_markers_.type = visualization_msgs::Marker::POINTS; + path_points_markers_.action = visualization_msgs::Marker::ADD; + path_points_markers_.pose.orientation.w = 1; + path_points_markers_.color.r = 0.0; + path_points_markers_.color.g = 1.0; + path_points_markers_.color.b = 1.0; + path_points_markers_.color.a = 1.0; + path_points_markers_.scale.x = _scale; + path_points_markers_.scale.y = _scale; + path_points_markers_.scale.z = _scale; + + } + void publishMarker(visualization_msgs::Marker &_marker, const ros::Publisher &_pub){ + + //Clear previous marker + if( !overlay_markers_ ){ + _marker.action = visualization_msgs::Marker::DELETEALL; + _pub.publish(_marker); + }else{ + path_points_markers_.id = rand(); + path_points_markers_.header.stamp = ros::Time::now(); + setRandomColor(path_points_markers_.color); + + path_line_markers_.id = rand(); + path_line_markers_.header.stamp = ros::Time::now(); + setRandomColor(path_line_markers_.color); + } + _marker.action = visualization_msgs::Marker::ADD; + _pub.publish(_marker); + } + void setRandomColor(std_msgs::ColorRGBA &_color, unsigned int _n_div = 20){ + //Using golden angle approximation + const double golden_angle = 180 * (3 - sqrt(5)); + double hue = color_id_ * golden_angle + 60; + color_id_++; + if(color_id_ == _n_div) + color_id_ = 1; + + auto random_color = Planners::Misc::HSVtoRGB(hue, 100, 100); + + _color.r = random_color.x; + _color.g = random_color.y; + _color.b = random_color.z; + } + + + ros::NodeHandle lnh_{"~"}; + ros::ServiceServer request_path_server_, change_planner_server_; + ros::Subscriber pointcloud_local_sub_, occupancy_grid_local_sub_; + //TODO Fix point markers + ros::Publisher line_markers_pub_, point_markers_pub_, cloud_test; + + tf::TransformListener m_tfListener; + + std::unique_ptr m_local_grid3d_; + + std::unique_ptr algorithm_; + + visualization_msgs::Marker path_line_markers_, path_points_markers_; + + //Parameters + Planners::utils::Vec3i world_size_; // Discrete + // JAC: local_world_size_meters: size of the local world provided by the launch + // JAC: local_world_size_: size of the discrete local world from local_world_size_meters (of the launch) and resolution + Planners::utils::Vec3i local_world_size_, local_world_size_meters; // Discrete + float resolution_; + + bool save_data_; + bool use3d_{true}; + + bool inflate_{false}; + unsigned int inflation_steps_{0}; + std::string data_folder_; + bool overlay_markers_{0}; + unsigned int color_id_{0}; + nav_msgs::OccupancyGrid occupancy_grid_; + pcl::PointCloud cloud_; + + //! Indicates that the local transfrom for the pointcloud is cached + bool m_tfCache; + tf::StampedTransform pclTf; + + //! Node parameters + // std::string m_inCloudTopic; + std::string baseFrameId; + std::string odomFrameId; + std::string globalFrameId; + // std::string m_baseFrameId; + + // Visualization of the map as pointcloud: measurements of 3D LIDAR + // sensor_msgs::PointCloud2 local_cloud; + //0: no map yet + //1: using occupancy + //2: using cloud + int input_map_{0}; + std::string heuristic_; + + int number_of_points; + bool mapReceived; + +}; +// int main(int argc, char **argv) +// { +// ros::init(argc, argv, "heuristic_local_planner_ros_node"); + +// HeuristicLocalPlannerROS heuristic_local_planner_ros; +// ros::spin(); + +// return 0; +// } + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "heuristic_local_planner_ros_node"); + + tf2_ros::Buffer tfBuffer; + tf2_ros::TransformListener tfListener(tfBuffer); + + HeuristicLocalPlannerROS heuristic_local_planner_ros; + + // f = boost::bind(&LocalPlanner::dynRecCb,&lcPlanner, _1, _2); + // server.setCallback(f); + + ros::Rate loop_rate(30); + while(ros::ok()){ + + ros::spinOnce(); + + // Call to Local Plan + heuristic_local_planner_ros.plan(); + + loop_rate.sleep(); + } + return 0; +} \ No newline at end of file