Merge branch 'melodic_migration' into 'master'

Changed the macro used to create new plugins.

See merge request !1

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 

config/default_config.yaml

 # 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

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>

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>

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);