update

include/robot_costmap_2d/costmap_2d_robot.h

@@ -55,18 +55,18 @@
 
 #include <robot/node_handle.h>
 #include <robot/plugin_loader_helper.h>
-class SuperValue : public robot::XmlRpc::XmlRpcValue
+class SuperValue : public robot_xmlrpcpp::XmlRpcValue
 {
 public:
-  void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a)
+  void setStruct(robot_xmlrpcpp::XmlRpcValue::ValueStruct* a)
   {
     _type = TypeStruct;
-    _value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a);
+    _value.asStruct = new robot_xmlrpcpp::XmlRpcValue::ValueStruct(*a);
   }
-  void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a)
+  void setArray(robot_xmlrpcpp::XmlRpcValue::ValueArray* a)
   {
     _type = TypeArray;
-    _value.asArray = new std::vector<robot::XmlRpc::XmlRpcValue>(*a);
+    _value.asArray = new std::vector<robot_xmlrpcpp::XmlRpcValue>(*a);
   }
 };
 

include/robot_costmap_2d/critical_layer.h

@@ -10,10 +10,6 @@ class CriticalLayer : public StaticLayer
 public:
   CriticalLayer();
   virtual ~CriticalLayer();
-  LayerType getType() const override
-  {
-      return LayerType::CRITICAL_LAYER;
-  }
 private:
   unsigned char interpretValue(unsigned char value) override;
   void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;

include/robot_costmap_2d/directional_layer.h

@@ -13,10 +13,7 @@ namespace robot_costmap_2d
     virtual ~DirectionalLayer();
     bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
     void resetMap();
-    LayerType getType() const override
+
-    {
-        return LayerType::DIRECTIONAL_LAYER;
-    }
 	private:
     void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
 		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);

include/robot_costmap_2d/footprint.h

@@ -136,7 +136,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandl
  * @param full_param_name this is the full name of the rosparam from
  * which the footprint_xmlrpc value came.  It is used only for
  * reporting errors. */
-std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
+std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& footprint_xmlrpc,
                                 const std::string& full_param_name);
 
 // /** @brief Write the current unpadded_footprint_ to the "footprint"

include/robot_costmap_2d/inflation_layer.h

@@ -67,7 +67,6 @@ public:
   unsigned int index_;
   unsigned int x_, y_;
   unsigned int src_x_, src_y_;
-  
 };
 
 class InflationLayer : public Layer
@@ -121,10 +120,6 @@ public:
    */
   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
 
-  LayerType getType() const override
-  {
-      return LayerType::INFLATION_LAYER;
-  }
 protected:
   virtual void onFootprintChanged();
   boost::recursive_mutex* inflation_access_;

include/robot_costmap_2d/layer.h

@@ -45,21 +45,6 @@
 #include <robot/node_handle.h>
 namespace robot_costmap_2d
 {
-enum class LayerType
-{
-    UNKNOWN,
-    STATIC_LAYER,
-    OBSTACLE_LAYER,
-    INFLATION_LAYER,
-    CRITICAL_LAYER,
-    DIRECTIONAL_LAYER,
-    PREFERRED_LAYER,
-    UNPREFERRED_LAYER,
-    VOXEL_LAYER
-};
-
-
-
 class LayeredCostmap;
 
 class Layer
@@ -94,7 +79,7 @@ public:
 
   virtual void reset() {}
 
-  virtual ~Layer() = default;
+  virtual ~Layer() {}
 
   /**
    * @brief Check to make sure all the data in the layer is up to date.
@@ -145,8 +130,6 @@ public:
    * notified of changes to the robot's footprint. */
   virtual void onFootprintChanged() {}
 
-  virtual LayerType getType() const { return LayerType::UNKNOWN; }
-
   template<typename T>
   void dataCallBack(const T& value, const std::string& topic) {
       handle(value, topic);

include/robot_costmap_2d/obstacle_layer.h

@@ -76,11 +76,6 @@ public:
   void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
-  LayerType getType() const override
-  {
-      return LayerType::OBSTACLE_LAYER;
-  }
-
 protected:
   void handleImpl(const void* data, 
                   const std::type_info&, 

include/robot_costmap_2d/preferred_layer.h

@@ -10,10 +10,6 @@ class PreferredLayer : public StaticLayer
 public:
   PreferredLayer();
   virtual ~PreferredLayer();
-  LayerType getType() const override
-  {
-      return LayerType::PREFERRED_LAYER;
-  }
 private:
   unsigned char interpretValue(unsigned char value);
 };

include/robot_costmap_2d/static_layer.h

@@ -63,12 +63,6 @@ public:
   virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
-  
-  LayerType getType() const override
-  {
-      return LayerType::STATIC_LAYER;
-  }
-
 protected:
   void handleImpl(const void* data,
                   const std::type_info& type,

include/robot_costmap_2d/unpreferred_layer.h

@@ -10,10 +10,7 @@ class UnPreferredLayer : public StaticLayer
 public:
   UnPreferredLayer();
   virtual ~UnPreferredLayer();
-  LayerType getType() const override
+
-  {
-      return LayerType::UNPREFERRED_LAYER;
-  }
 private:
   unsigned char interpretValue(unsigned char value);
 

include/robot_costmap_2d/voxel_layer.h

@@ -76,10 +76,7 @@ public:
   }
   virtual void matchSize();
   virtual void reset();
-  LayerType getType() const override
+
-  {
-      return LayerType::VOXEL_LAYER;
-  }
 
 protected:
 

plugins/static_layer.cpp

@@ -347,7 +347,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
     tf3::TransformStampedMsg transformMsg;
     try
     {
-      transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time());
+      transformMsg = tf_->lookupTransform(map_frame_, global_frame_, tf3::Time::now());
     }
     catch (tf3::TransformException ex)
     {

src/footprint.cpp

@@ -249,25 +249,25 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromParams(robot::NodeHandl
 //   nh.setParam("footprint", oss.str().c_str());
 // }
 
-double getNumberFromXMLRPC(robot::XmlRpc::XmlRpcValue& value, const std::string& full_param_name)
+double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string& full_param_name)
 {
   // Make sure that the value we're looking at is either a double or an int.
-  if (value.getType() != robot::XmlRpc::XmlRpcValue::TypeInt &&
+  if (value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeInt &&
-      value.getType() != robot::XmlRpc::XmlRpcValue::TypeDouble)
+      value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble)
   {
     std::string& value_string = value;
     printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
                full_param_name.c_str(), value_string.c_str());
     throw std::runtime_error("Values in the footprint specification must be numbers");
   }
-  return value.getType() == robot::XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
+  return value.getType() == robot_xmlrpcpp::XmlRpcValue::TypeInt ? (int)(value) : (double)(value);
 }
 
-std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::XmlRpcValue& footprint_xmlrpc,
+std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& footprint_xmlrpc,
                                 const std::string& full_param_name)
 {
   // Make sure we have an array of at least 3 elements.
-  if (footprint_xmlrpc.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
+  if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
       footprint_xmlrpc.size() < 3)
   {
     printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
@@ -282,8 +282,8 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot::XmlRpc::X
   for (int i = 0; i < footprint_xmlrpc.size(); ++i)
   {
     // Make sure each element of the list is an array of size 2. (x and y coordinates)
-    robot::XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
+    robot_xmlrpcpp::XmlRpcValue point = footprint_xmlrpc[ i ];
-    if (point.getType() != robot::XmlRpc::XmlRpcValue::TypeArray ||
+    if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
         point.size() != 2)
     {
       printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "

src/observation_buffer.cpp

@@ -66,7 +66,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
   std::string tf_error;
 
   robot_geometry_msgs::TransformStamped transformStamped;
-  if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time(), &tf_error))
+  if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
   {
     printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
               global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@@ -90,7 +90,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
       tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
                         new_global_frame,                      // frame đích
                         origin.header.frame_id,                      // frame nguồn
-                        tf3::Time()
+                        transform_time
                         );
       tf3::doTransform(origin, origin, tfm_1);
       obs.origin_ = origin.point;
@@ -100,7 +100,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
       tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
                         new_global_frame,                      // frame đích
                         obs.cloud_->header.frame_id,                      // frame nguồn
-                        tf3::Time()
+                        transform_time
                         );
       tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tfm_2);
     }
@@ -140,7 +140,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         local_origin.header.frame_id,                      // frame nguồn
-                        tf3::Time()
+                        data_convert::convertTime(local_origin.header.stamp)
                         );
     tf3::doTransform(local_origin, global_origin, tfm_1);
 
@@ -161,7 +161,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         cloud.header.frame_id,                      // frame nguồn
-                        tf3::Time()
+                        data_convert::convertTime(cloud.header.stamp)
                         );
     tf3::doTransform(cloud, global_frame_cloud, tfm_2);
     global_frame_cloud.header.stamp = cloud.header.stamp;