diff --git a/CMakeLists.txt b/CMakeLists.txt index 966c9d298dbbddcc2199c57cc8c5d1d7f627c22f..3a6d262a3cd891de315c206baa7179b0cfdfed87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,7 +104,6 @@ target_link_libraries(${PROJECT_NAME}_nodelet ${OpenCV_LIBS}) # Add message headers dependencies # ******************************************************************** # add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp) -add_dependencies(${PROJECT_NAME}_nodelet cv_bridge_generate_messages_cpp) add_dependencies(${PROJECT_NAME}_nodelet sensor_msgs_generate_messages_cpp) # ******************************************************************** # Add dynamic reconfigure dependencies diff --git a/config/default_config.yaml b/config/default_config.yaml index f08a43db88ba04175d07510188f55cc2608ec3ad..bf059a19747afa22406cbf7f717bae5843d8869b 100755 --- a/config/default_config.yaml +++ b/config/default_config.yaml @@ -1,5 +1,5 @@ # sensor information -base_frame_id: "ana/base_footprint" +#base_frame_id: "ana/base_footprint" min_range: 0.1 max_range: 50.0 sensor_height: 0.3 diff --git a/launch/node.launch b/launch/node.launch index b65000d43b3918c6d715ec63e9c175c2555afe9b..52a0653e55f2111e226d5e38f37f606e844fde98 100644 --- a/launch/node.launch +++ b/launch/node.launch @@ -7,6 +7,7 @@ <arg name="output" default="screen"/> <arg name="launch_prefix" default=""/> <arg name="cloud_in" default="~pointcloud_in"/> + <arg name="base_frame_id" default="$(arg ns)/base_footprint"/> <group ns="$(arg ns)"> @@ -17,6 +18,7 @@ launch-prefix="$(arg launch_prefix)"> <remap from="~pointcloud_in" to="$(arg cloud_in)"/> <rosparam file="$(arg config_file)" command="load"/> + <param name="base_frame_id" value="$(arg base_frame_id)"/> </node> </group> diff --git a/launch/nodelet.launch b/launch/nodelet.launch index 9926783b94d5cd41a29cb504c25cf8addae50106..dbdc16d2dc3afa178e1b1e47b4a3d70db9348e36 100644 --- a/launch/nodelet.launch +++ b/launch/nodelet.launch @@ -8,6 +8,7 @@ <arg name="output" default="screen"/> <arg name="launch_prefix" default=""/> <arg name="cloud_in" default="~pointcloud_in"/> + <arg name="base_frame_id" default="$(arg ns)/base_footprint"/> <group ns="$(arg ns)"> @@ -18,6 +19,7 @@ output="$(arg output)"> <remap from="~pointcloud_in" to="$(arg cloud_in)"/> <rosparam file="$(arg config_file)" command="load"/> + <param name="base_frame_id" value="$(arg base_frame_id)"/> </node> </group> diff --git a/src/lidar_obstacle_detector_alg_node.cpp b/src/lidar_obstacle_detector_alg_node.cpp index 3f7347a39b044712810752c1da099e84bc94bf63..f7f25270c7d2617c5ec822f2f8548ee1d06e1a34 100644 --- a/src/lidar_obstacle_detector_alg_node.cpp +++ b/src/lidar_obstacle_detector_alg_node.cpp @@ -46,7 +46,10 @@ bool LidarObstacleDetectorAlgNode::get_point(unsigned int ring, unsigned int col } else { - point=this->transformed_input->at(ring,col); + if (this->transformed_input->width > this->transformed_input->height) + point=this->transformed_input->at(col,ring); + else + point=this->transformed_input->at(ring,col); if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z)) { this->lidar_data[col*this->config_.max_ring+ring].valid=true; @@ -96,6 +99,7 @@ double LidarObstacleDetectorAlgNode::get_max_inclination(double current_angle,pc void LidarObstacleDetectorAlgNode::mainNodeThread(void) { + bool ring_ordered = false; static bool success; static std::vector<float> farthest_ranges,virtual_ranges; static double farthest_range,virtual_range; @@ -151,6 +155,8 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) this->alg_.unlock(); return; } + if (this->transformed_input->width > this->transformed_input->height) + ring_ordered = true; if(this->transformed_input->height==1 || this->transformed_input->width==1) { ROS_WARN("Droping pointcloud because it is not organized"); @@ -178,13 +184,13 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) this->scan_out_LaserScan_msg_.header=this->input_header; this->scan_out_LaserScan_msg_.angle_min=-3.14159; this->scan_out_LaserScan_msg_.angle_max=3.14159; - this->scan_out_LaserScan_msg_.angle_increment=(2.0*3.14159-start_angle_inc)/this->transformed_input->height; + this->scan_out_LaserScan_msg_.angle_increment=(2.0*3.14159-start_angle_inc)/(ring_ordered? this->transformed_input->width: this->transformed_input->height); this->scan_out_LaserScan_msg_.ranges.clear(); this->scan_out_LaserScan_msg_.range_min=this->config_.min_range; this->scan_out_LaserScan_msg_.range_max=this->config_.max_range; max_col=(unsigned int)((2.0*3.14159)/this->scan_out_LaserScan_msg_.angle_increment); - if(max_col>this->transformed_input->height) - max_col=this->transformed_input->height; + if(max_col>(ring_ordered? this->transformed_input->width: this->transformed_input->height)) + max_col=(ring_ordered? this->transformed_input->width: this->transformed_input->height); this->inc_image.create(this->config_.max_ring,max_col,CV_8UC1); this->label_image.create(this->config_.max_ring,max_col,CV_32SC1); @@ -219,7 +225,10 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) prev_ring=-1; prev_ring_point_valid=true; - point=this->transformed_input->at(0,col); + if (ring_ordered) + point=this->transformed_input->at(col,0); + else + point=this->transformed_input->at(0,col); if(!isnan(point.x) && !isnan(point.y) && !isnan(point.z)) { old_angle=angle; @@ -249,7 +258,7 @@ void LidarObstacleDetectorAlgNode::mainNodeThread(void) if((next_col_point_valid=this->get_point(ring,next_col,next_col_point,next_col_range))==true) { col_range=sqrt(pow(point.x-next_col_point.x,2.0)+pow(point.y-next_col_point.y,2.0)); - this->lidar_data[col*this->config_.max_ring+ring].col_range=col_range; + this->lidar_data[col*this->config_.max_ring+ring].col_range=col_range;//next_col???????????????? } if(prev_ring==-1 || prev_ring_point_valid) { @@ -519,11 +528,11 @@ void *LidarObstacleDetectorAlgNodelet::spin_thread(void *param) { LidarObstacleDetectorAlgNodelet *nodelet=(LidarObstacleDetectorAlgNodelet *)param; - nodelet->node->spin(); + nodelet->node->nodelet_spin(); pthread_exit(NULL); } // parameters are: package, class name, class type, base class type -PLUGINLIB_DECLARE_CLASS(iri_lidar_obstacle_detector, LidarObstacleDetectorAlgNodelet, LidarObstacleDetectorAlgNodelet, nodelet::Nodelet); +PLUGINLIB_EXPORT_CLASS(LidarObstacleDetectorAlgNodelet, nodelet::Nodelet);