Skip to content
Snippets Groups Projects
Commit 4ce32cd2 authored by Iván del Pino's avatar Iván del Pino
Browse files

renamed things to make the node more general (instead of velodyne, now using lidar)

parent 2ae90c88
No related branches found
No related tags found
No related merge requests found
......@@ -43,37 +43,31 @@ class GroundSegmentationAlgNode : public algorithm_base::IriBaseAlgorithm<
GroundSegmentationAlgorithm>
{
private:
std::string input_pointcloud_topic_;
std::string output_pointcloud_segmented_topic_;
std::string output_roadmap_markers_topic_;
std::string output_roadmap_nodes_topic_;
std::string output_roadmap_edges_topic_;
std_msgs::Header common_header_;
bool flag_new_velodyne_data_;
ros::Publisher velodyne_republisher_;
bool flag_new_lidar_data_;
ros::Publisher lidar_points_ground_segmented_publisher_;
ros::Publisher local_goals_publisher_;
visualization_msgs::MarkerArray local_goals_MarkerArray_msg_;
ros::Publisher roadmap_markers_publisher_;
visualization_msgs::MarkerArray roadmap_MarkerArray_msg_;
// [subscriber attributes]
// Variables for copying the callbacks inputs
sensor_msgs::PointCloud2 velodyne_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> velodyne_pcl_cloud_;
sensor_msgs::PointCloud2 lidar_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> lidar_pcl_cloud_;
ros::Publisher ground_references_publisher_;
sensor_msgs::PointCloud2 ground_references_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> ground_references_pcl_cloud_;
ros::Publisher roadmap_nodes_publisher_;
sensor_msgs::PointCloud2 roadmap_nodes_ros_cloud_;
pcl::PointCloud<pcl::PointXYZRGBNormal> roadmap_nodes_pcl_cloud_;
ros::Publisher edges_publisher_;
iri_ground_segmentation::edges_array edges_array_;
ros::Publisher roadmap_edges_publisher_;
iri_ground_segmentation::edges_array roadmap_edges_array_;
ros::Subscriber velodyne_subscriber_;
void cb_velodyne(const sensor_msgs::PointCloud2::ConstPtr& msg);
ros::Subscriber lidar_subscriber_;
void cb_lidar(const sensor_msgs::PointCloud2::ConstPtr& msg);
pthread_mutex_t velodyne_mutex_;
void velodyne_mutex_enter(void);
void velodyne_mutex_exit(void);
pthread_mutex_t lidar_mutex_;
void lidar_mutex_enter(void);
void lidar_mutex_exit(void);
void rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2& ros_point_cloud,
pcl::PointCloud<pcl::PointXYZRGBNormal>& pcl_point_cloud);
......
......@@ -11,70 +11,33 @@ GroundSegmentationAlgNode::GroundSegmentationAlgNode(void) :
else
this->setRate(this->config_.rate);
if (!this->private_node_handle_.getParam("input_pointcloud_topic", this->config_.input_pointcloud_topic))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'input_pointcloud_topic' not found");
}
else
input_pointcloud_topic_ = config_.input_pointcloud_topic;
if (!this->private_node_handle_.getParam("output_pointcloud_segmented_topic",
this->config_.output_pointcloud_segmented_topic))
{
ROS_WARN(
"GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_pointcloud_segmented_topic' not found");
}
else
output_pointcloud_segmented_topic_ = config_.output_pointcloud_segmented_topic;
if (!this->private_node_handle_.getParam("output_roadmap_markers_topic", this->config_.output_roadmap_markers_topic))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_markers' not found");
}
else
output_roadmap_markers_topic_ = config_.output_roadmap_markers_topic;
if (!this->private_node_handle_.getParam("output_roadmap_nodes_topic", this->config_.output_roadmap_nodes_topic))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_nodes_topic' not found");
}
else
output_roadmap_nodes_topic_ = config_.output_roadmap_nodes_topic;
if (!this->private_node_handle_.getParam("output_roadmap_edges_topic", this->config_.output_roadmap_edges_topic))
{
ROS_WARN("GroundSegmentationAlgNode::GroundSegmentationAlgNode: param 'output_roadmap_edges_topic' not found");
}
else
output_roadmap_edges_topic_ = config_.output_roadmap_edges_topic;
flag_new_velodyne_data_ = false;
flag_new_lidar_data_ = false;
// [init publishers]
this->velodyne_republisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
> (output_pointcloud_segmented_topic_, 1);
this->lidar_points_ground_segmented_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
> ("lidar_points_ground_segmented", 1);
this->local_goals_publisher_ = this->private_node_handle_.advertise < visualization_msgs::MarkerArray
> (output_roadmap_markers_topic_, 1);
this->roadmap_markers_publisher_ = this->private_node_handle_.advertise < visualization_msgs::MarkerArray
> ("roadmap_markers", 1);
this->ground_references_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
> (output_roadmap_nodes_topic_, 1);
this->roadmap_nodes_publisher_ = this->private_node_handle_.advertise < sensor_msgs::PointCloud2
> ("roadmap_nodes", 1);
this->edges_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::edges_array
> (output_roadmap_edges_topic_, 1);
this->roadmap_edges_publisher_ = this->private_node_handle_.advertise < iri_ground_segmentation::edges_array
> ("roadmap_edges", 1);
// [init subscribers]
this->velodyne_subscriber_ = this->public_node_handle_.subscribe(input_pointcloud_topic_, 1,
&GroundSegmentationAlgNode::cb_velodyne, this);
this->lidar_subscriber_ = this->public_node_handle_.subscribe("/lidar_points_extended_structure", 1,
&GroundSegmentationAlgNode::cb_lidar, this);
pthread_mutex_init(&this->velodyne_mutex_, NULL);
pthread_mutex_init(&this->lidar_mutex_, NULL);
// [init services]
}
GroundSegmentationAlgNode::~GroundSegmentationAlgNode(void)
{
this->showPerformanceStatistics();
pthread_mutex_destroy(&this->velodyne_mutex_);
pthread_mutex_destroy(&this->lidar_mutex_);
}
void GroundSegmentationAlgNode::rosPointCloudtoPCLPointCloud(const sensor_msgs::PointCloud2 &ros_point_cloud,
......@@ -95,19 +58,19 @@ void GroundSegmentationAlgNode::PCLPointCloudToRosPointCloud(
void GroundSegmentationAlgNode::mutexAll(void)
{
this->velodyne_mutex_enter();
this->lidar_mutex_enter();
}
void GroundSegmentationAlgNode::unmutexAll(void)
{
this->velodyne_mutex_exit();
this->lidar_mutex_exit();
}
bool GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived(void)
{
bool ready_to_process = false;
if (flag_new_velodyne_data_)
if (flag_new_lidar_data_)
{
ready_to_process = true;
//std::cout << "GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived..." << std::endl;
......@@ -118,46 +81,46 @@ bool GroundSegmentationAlgNode::isAllRequiredDataAlreadyReceived(void)
void GroundSegmentationAlgNode::storeHeaderForSync(void)
{
common_header_ = velodyne_ros_cloud_.header;
common_header_ = lidar_ros_cloud_.header;
}
void GroundSegmentationAlgNode::convertInputsToSuitableFormats(void)
{
//std::cout << "GroundSegmentationAlgNode::convertInputsToSuitableFormats..." << std::endl;
//std::cout << " 1) rosPointCloudtoPCLPointCloud with velodyne_ros_cloud_..." << std::endl;
this->rosPointCloudtoPCLPointCloud(velodyne_ros_cloud_, velodyne_pcl_cloud_);
flag_new_velodyne_data_ = false;
//std::cout << " 1) rosPointCloudtoPCLPointCloud with lidar_ros_cloud_..." << std::endl;
this->rosPointCloudtoPCLPointCloud(lidar_ros_cloud_, lidar_pcl_cloud_);
flag_new_lidar_data_ = false;
}
void GroundSegmentationAlgNode::convertOutputsToSuitableFormats(void)
{
//std::cout << "Extracted header = " << std::endl << common_header_;
//std::cout << "Original header = " << std::endl << velodyne_ros_cloud_.header << std::endl;
//std::cout << "Original header = " << std::endl << lidar_ros_cloud_.header << std::endl;
//std::cout << "Message header = " << std::endl << perimeters_ros_.header << std::endl;
this->PCLPointCloudToRosPointCloud(velodyne_pcl_cloud_, velodyne_ros_cloud_);
velodyne_ros_cloud_.header = common_header_;
this->PCLPointCloudToRosPointCloud(lidar_pcl_cloud_, lidar_ros_cloud_);
lidar_ros_cloud_.header = common_header_;
this->PCLPointCloudToRosPointCloud(ground_references_pcl_cloud_, ground_references_ros_cloud_);
ground_references_ros_cloud_.header = common_header_;
this->PCLPointCloudToRosPointCloud(roadmap_nodes_pcl_cloud_, roadmap_nodes_ros_cloud_);
roadmap_nodes_ros_cloud_.header = common_header_;
}
void GroundSegmentationAlgNode::publishAll(void)
{
//std::cout << "Starting GroundSegmentationAlgNode::publishAll.." << std::endl;
this->velodyne_republisher_.publish(this->velodyne_ros_cloud_);
this->ground_references_publisher_.publish(this->ground_references_ros_cloud_);
this->local_goals_publisher_.publish(this->local_goals_MarkerArray_msg_);
this->edges_publisher_.publish(this->edges_array_);
this->lidar_points_ground_segmented_publisher_.publish(this->lidar_ros_cloud_);
this->roadmap_nodes_publisher_.publish(this->roadmap_nodes_ros_cloud_);
this->roadmap_markers_publisher_.publish(this->roadmap_MarkerArray_msg_);
this->roadmap_edges_publisher_.publish(this->roadmap_edges_array_);
}
void GroundSegmentationAlgNode::prepareForNextIteration(void)
{
velodyne_pcl_cloud_.clear();
ground_references_pcl_cloud_.clear();
local_goals_MarkerArray_msg_.markers.clear();
edges_array_.edges_array.clear();
lidar_pcl_cloud_.clear();
roadmap_nodes_pcl_cloud_.clear();
roadmap_MarkerArray_msg_.markers.clear();
roadmap_edges_array_.edges_array.clear();
}
void GroundSegmentationAlgNode::showPerformanceStatistics(void)
......@@ -180,8 +143,8 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
this->storeHeaderForSync();
this->convertInputsToSuitableFormats();
this->alg_.segmentGround(velodyne_pcl_cloud_, ground_references_pcl_cloud_, local_goals_MarkerArray_msg_,
edges_array_, common_header_);
this->alg_.segmentGround(lidar_pcl_cloud_, roadmap_nodes_pcl_cloud_, roadmap_MarkerArray_msg_, roadmap_edges_array_,
common_header_);
this->convertOutputsToSuitableFormats();
this->publishAll();
......@@ -194,25 +157,25 @@ void GroundSegmentationAlgNode::mainNodeThread(void)
/* [subscriber callbacks] */
void GroundSegmentationAlgNode::cb_velodyne(const sensor_msgs::PointCloud2::ConstPtr &msg)
void GroundSegmentationAlgNode::cb_lidar(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
//this->velodyne_mutex_enter();
velodyne_ros_cloud_ = *msg;
//std::cout << "GroundSegmentationAlgNode::cb_velodyne --> Velodyne msg received!" << std::endl;
assert(msg != NULL && "Null pointer!!! in function cb_velodyne!");
//this->lidar_mutex_enter();
lidar_ros_cloud_ = *msg;
//std::cout << "GroundSegmentationAlgNode::cb_lidar --> lidar msg received!" << std::endl;
assert(msg != NULL && "Null pointer!!! in function cb_lidar!");
flag_new_velodyne_data_ = true;
//this->velodyne_mutex_exit();
flag_new_lidar_data_ = true;
//this->lidar_mutex_exit();
}
void GroundSegmentationAlgNode::velodyne_mutex_enter(void)
void GroundSegmentationAlgNode::lidar_mutex_enter(void)
{
pthread_mutex_lock(&this->velodyne_mutex_);
pthread_mutex_lock(&this->lidar_mutex_);
}
void GroundSegmentationAlgNode::velodyne_mutex_exit(void)
void GroundSegmentationAlgNode::lidar_mutex_exit(void)
{
pthread_mutex_unlock(&this->velodyne_mutex_);
pthread_mutex_unlock(&this->lidar_mutex_);
}
/* [service callbacks] */
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment