update for melodic

wpb_home_tutorials/nav_lidar/costmap_common_params.yaml

@@ -1,7 +1,7 @@
 obstacle_range: 6.0
 raytrace_range: 6.0
 robot_radius: 0.25
-inflation_radius: 0.40
+inflation_radius: 0.30
 cost_scaling_factor: 10.0
 observation_sources: scan_filtered
 scan_filtered: {

wpb_home_tutorials/nav_lidar/global_costmap_params.yaml

@@ -1,6 +1,6 @@
 global_costmap:
-  global_frame: /map
-  robot_base_frame: /base_footprint
+  global_frame: map
+  robot_base_frame: base_footprint
   update_frequency: 1.0
   static_map: true
   transform_tolerance: 1.0

wpb_home_tutorials/nav_lidar/local_costmap_params.yaml

@@ -1,11 +1,15 @@
 local_costmap:
-  global_frame: /odom
-  robot_base_frame: /base_footprint
+  global_frame: odom
+  robot_base_frame: base_footprint
   update_frequency: 5.0
   publish_frequency: 3.0
   static_map: false
   rolling_window: true
-  width: 5.0
-  height: 5.0
+  width: 3.0
+  height: 3.0
   resolution: 0.05
-  transform_tolerance: 0.1
+  transform_tolerance: 0.1
+  plugins:
+    - {name: static_layer,    type: "costmap_2d::StaticLayer"}
+    - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
+    - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

wpbh_local_planner/include/wpbh_local_planner/wpbh_local_planner.h

@@ -51,6 +51,7 @@
 #include <tf/tf.h>
 #include <tf/transform_listener.h>
 #include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
 
 #define WPBH_STEP_GOTO     1
 #define WPBH_STEP_NEAR     2
@@ -65,6 +66,7 @@ namespace wpbh_local_planner
     ~WpbhLocalPlanner();
 
     void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros);
+    void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
     bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan);
     bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
     bool isGoalReached();

wpbh_local_planner/src/wpbh_local_planner.cpp

@@ -42,7 +42,8 @@
 #include <wpbh_local_planner/CLidarAC.h>
 
 // register this planner as a WpbhLocalPlanner plugin
-PLUGINLIB_DECLARE_CLASS(wpbh_local_planner, WpbhLocalPlanner, wpbh_local_planner::WpbhLocalPlanner, nav_core::BaseLocalPlanner)
+//PLUGINLIB_DECLARE_CLASS(wpbh_local_planner, WpbhLocalPlanner, wpbh_local_planner::WpbhLocalPlanner, nav_core::BaseLocalPlanner)
+PLUGINLIB_EXPORT_CLASS( wpbh_local_planner::WpbhLocalPlanner, nav_core::BaseLocalPlanner)
 
 static CLidarAC lidar_ac;
 static float ranges[360];
@@ -63,6 +64,7 @@ namespace wpbh_local_planner
 
     WpbhLocalPlanner::~WpbhLocalPlanner()
     {
+        
     }
 
     void WpbhLocalPlanner::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
@@ -102,6 +104,44 @@ namespace wpbh_local_planner
         }
     }
 
+    void WpbhLocalPlanner::initialize(std::string name,tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros)
+    {
+        ROS_WARN("WpbhLocalPlanner::initialize() ");
+        if(!m_bInitialized)
+        {	
+            //m_tf_listener = tf;
+            m_tf_listener = new tf::TransformListener;
+            m_costmap_ros = costmap_ros;
+            m_costmap = m_costmap_ros->getCostmap();
+            
+            m_global_frame_id = m_costmap_ros->getGlobalFrameID();      //"odom"
+            m_robot_base_frame_id = m_costmap_ros->getBaseFrameID();    //"base_footprint"
+            
+            m_footprint_spec = m_costmap_ros->getRobotFootprint();
+            costmap_2d::calculateMinAndMaxDistances(m_footprint_spec, m_robot_inscribed_radius, m_robot_circumscribed_radius); 
+
+            ros::NodeHandle nh_planner("~/" + name);
+            nh_planner.param("max_vel_trans", m_max_vel_trans, 0.3);
+            nh_planner.param("max_vel_rot", m_max_vel_rot, 0.9);
+            nh_planner.param("acc_scale_trans", m_acc_scale_trans, 1.0);
+            nh_planner.param("acc_scale_rot", m_acc_scale_rot, 0.5);
+            nh_planner.param("goal_dist_tolerance", m_goal_dist_tolerance, 0.1);
+            nh_planner.param("goal_yaw_tolerance", m_goal_yaw_tolerance, 0.1);
+            nh_planner.param("scan_topic", m_scan_topic, std::string("/scan"));
+
+            m_pub_target = nh_planner.advertise<geometry_msgs::PoseStamped>("local_planner_target", 10);
+            m_scan_sub = nh_planner.subscribe<sensor_msgs::LaserScan>(m_scan_topic,1,&WpbhLocalPlanner::lidarCallback,this);
+            
+            m_bInitialized = true;
+
+            ROS_DEBUG("wpbh_local_planner plugin initialized.");
+        }
+        else
+        {
+            ROS_WARN("wpbh_local_planner has already been initialized, doing nothing.");
+        }
+    }
+
     static double fScaleD2R = 3.14159 / 180;
     void WpbhLocalPlanner::lidarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
     {