diff --git a/include/robot_costmap_2d/critical_layer.h b/include/robot_costmap_2d/critical_layer.h index c31bd62..85e3645 100644 --- a/include/robot_costmap_2d/critical_layer.h +++ b/include/robot_costmap_2d/critical_layer.h @@ -10,6 +10,12 @@ 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; diff --git a/include/robot_costmap_2d/directional_layer.h b/include/robot_costmap_2d/directional_layer.h index 86fac6c..73b7ae9 100644 --- a/include/robot_costmap_2d/directional_layer.h +++ b/include/robot_costmap_2d/directional_layer.h @@ -14,6 +14,11 @@ namespace robot_costmap_2d bool laneFilter(const std::vector 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> new_map, const robot_nav_msgs::Path path); diff --git a/include/robot_costmap_2d/inflation_layer.h b/include/robot_costmap_2d/inflation_layer.h index 50eed86..a8fb5c2 100755 --- a/include/robot_costmap_2d/inflation_layer.h +++ b/include/robot_costmap_2d/inflation_layer.h @@ -120,6 +120,11 @@ 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_; diff --git a/include/robot_costmap_2d/layer.h b/include/robot_costmap_2d/layer.h index bcff269..0a9dbc2 100755 --- a/include/robot_costmap_2d/layer.h +++ b/include/robot_costmap_2d/layer.h @@ -45,6 +45,20 @@ #include 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 @@ -134,6 +148,9 @@ public: void dataCallBack(const T& value, const std::string& topic) { handle(value, topic); } + + virtual LayerType getType() const { return LayerType::UNKNOWN; } + protected: // Hàm template public, dùng để gửi dữ liệu @@ -157,7 +174,7 @@ protected: bool enabled_; std::string name_; tf3::BufferCore *tf_; - + private: std::vector footprint_spec_; }; diff --git a/include/robot_costmap_2d/obstacle_layer.h b/include/robot_costmap_2d/obstacle_layer.h index bce1c3b..d56f3ae 100755 --- a/include/robot_costmap_2d/obstacle_layer.h +++ b/include/robot_costmap_2d/obstacle_layer.h @@ -57,6 +57,7 @@ namespace robot_costmap_2d { struct CallBackInfo { + std::string observation_source; std::string data_type; std::string topic; bool inf_is_valid; @@ -84,6 +85,11 @@ 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&, diff --git a/include/robot_costmap_2d/preferred_layer.h b/include/robot_costmap_2d/preferred_layer.h index e1252af..80977e6 100644 --- a/include/robot_costmap_2d/preferred_layer.h +++ b/include/robot_costmap_2d/preferred_layer.h @@ -10,6 +10,12 @@ class PreferredLayer : public StaticLayer public: PreferredLayer(); virtual ~PreferredLayer(); + + LayerType getType() const override + { + return LayerType::PREFERRED_LAYER; + } + private: unsigned char interpretValue(unsigned char value); }; diff --git a/include/robot_costmap_2d/static_layer.h b/include/robot_costmap_2d/static_layer.h index 3df4a59..b766883 100755 --- a/include/robot_costmap_2d/static_layer.h +++ b/include/robot_costmap_2d/static_layer.h @@ -63,6 +63,12 @@ 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, diff --git a/include/robot_costmap_2d/unpreferred_layer.h b/include/robot_costmap_2d/unpreferred_layer.h index 1da5a86..cf62596 100644 --- a/include/robot_costmap_2d/unpreferred_layer.h +++ b/include/robot_costmap_2d/unpreferred_layer.h @@ -11,6 +11,11 @@ public: UnPreferredLayer(); virtual ~UnPreferredLayer(); + LayerType getType() const override + { + return LayerType::UNPREFERRED_LAYER; + } + private: unsigned char interpretValue(unsigned char value); diff --git a/include/robot_costmap_2d/voxel_layer.h b/include/robot_costmap_2d/voxel_layer.h index 3bdcf8c..da08c78 100755 --- a/include/robot_costmap_2d/voxel_layer.h +++ b/include/robot_costmap_2d/voxel_layer.h @@ -77,6 +77,10 @@ public: virtual void matchSize(); virtual void reset(); + LayerType getType() const override + { + return LayerType::VOXEL_LAYER; + } protected: diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index dc2f58d..04c3fce 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -198,7 +198,8 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m return; // make sure the inflation list is empty at the beginning of the cycle (should always be true) - printf("The inflation list must be empty at the beginning of inflation\n"); + if(!inflation_cells_.empty()) + printf("The inflation list must be empty at the beginning of inflation\n"); unsigned char* master_array = master_grid.getCharMap(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 6c4604b..cc4b2b0 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -165,6 +165,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa } CallBackInfo info_tmp; + info_tmp.observation_source = source; info_tmp.data_type = data_type; info_tmp.topic = topic; info_tmp.inf_is_valid = inf_is_valid; @@ -237,6 +238,8 @@ void ObstacleLayer::handleImpl(const void* data, topic == callback_infos_[i].topic && !callback_infos_[i].inf_is_valid) { + // if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i); + // if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i); laserScanCallback(*static_cast(data), buffer); } else if (type == typeid(robot_sensor_msgs::LaserScan) && @@ -244,6 +247,7 @@ void ObstacleLayer::handleImpl(const void* data, topic == callback_infos_[i].topic && callback_infos_[i].inf_is_valid) { + laserScanValidInfCallback(*static_cast(data), buffer); } else if (type == typeid(robot_sensor_msgs::PointCloud) && @@ -266,10 +270,14 @@ void ObstacleLayer::handleImpl(const void* data, } pointCloud2Callback(*static_cast(data), buffer); } - else - { - std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl; - } + // else + // { + // std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl + // << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl + // << "data type: " << callback_infos_[i].data_type << std::endl + // << "topic: " << topic << std::endl + // << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; + // } } } else diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp index 26599ef..dccda0b 100755 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -255,6 +255,8 @@ void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my) { my = (int)((wy - origin_y_) / resolution_); } + // printf("CHECK FUNCTION: %f | %f\n",wx,wy); + // printf("CHECK FUNCTION: resolution_: %f\n",resolution_); } void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y) diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 4b8f876..5ff0f34 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -128,6 +128,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH robot::PluginLoaderHelper loader; + if (nh.hasParam("rolling_window")) nh.getParam("rolling_window", rolling_window); @@ -395,6 +396,7 @@ void Costmap2DROBOT::updateMap() double x = pose.pose.position.x, y = pose.pose.position.y, yaw = data_convert::getYaw(pose.pose.orientation); + // robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw); layered_costmap_->updateMap(x, y, yaw); robot_geometry_msgs::PolygonStamped footprint; footprint.header.frame_id = global_frame_; @@ -498,6 +500,13 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) { robot_geometry_msgs::PoseStamped robot_pose; robot_geometry_msgs::Pose pose_default; + pose_default.orientation.x = 0; + pose_default.orientation.y = 0; + pose_default.orientation.z = 0; + pose_default.orientation.w = 1; + pose_default.position.x = 0; + pose_default.position.y = 0; + pose_default.position.z = 0; global_pose.pose = pose_default; robot_pose.pose = pose_default; @@ -509,10 +518,22 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) try { // use current time if possible (makes sure it's not in the future) - if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time))) + if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time())) { - tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)); + tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time()); tf3::doTransform(robot_pose, global_pose, transform); + // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(), + // global_pose.pose.position.x, + // global_pose.pose.position.y, + // global_pose.pose.position.z, + // global_pose.pose.orientation.x, + // global_pose.pose.orientation.y, + // global_pose.pose.orientation.z, + // global_pose.pose.orientation.w); + // transform.transform.rotation.x, + // transform.transform.rotation.y, + // transform.transform.rotation.z, + // transform.transform.rotation.w); } // use the latest otherwise else @@ -521,7 +542,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose) tf3::TransformStampedMsg transform = tf_.lookupTransform( global_frame_, // frame đích robot_base_frame_, // frame nguồn - data_convert::convertTime(robot_pose.header.stamp) + tf3::Time() ); tf3::doTransform(robot_pose, global_pose, transform); } diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index a66dbaa..46dd858 100755 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -111,7 +111,7 @@ namespace robot_costmap_2d minx_ = miny_ = 1e30; maxx_ = maxy_ = -1e30; - + printf("START\n"); for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) { @@ -121,6 +121,7 @@ namespace robot_costmap_2d double prev_miny = miny_; double prev_maxx = maxx_; double prev_maxy = maxy_; + std::cout << "robot x: " << robot_x << "\nrobot y: " << robot_y << "\nrobot yaw: " << robot_yaw << std::endl; (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { @@ -131,6 +132,7 @@ namespace robot_costmap_2d (*plugin)->getName().c_str()); } } + printf("END\n"); int x0, xn, y0, yn; costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0); @@ -141,7 +143,8 @@ namespace robot_costmap_2d y0 = std::max(0, y0); yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); - printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn); + // printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn); + // printf("Updating area x: [%f, %f] y: [%f, %f]\n", minx_, miny_, maxx_, maxy_); if (xn < x0 || yn < y0) return; diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index a6fbcad..77d6d6c 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -140,7 +140,8 @@ 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 - data_convert::convertTime(local_origin.header.stamp) + tf3::Time() + // data_convert::convertTime(local_origin.header.stamp) ); tf3::doTransform(local_origin, global_origin, tfm_1); @@ -161,7 +162,8 @@ 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 - data_convert::convertTime(cloud.header.stamp) + tf3::Time() + // data_convert::convertTime(cloud.header.stamp) ); tf3::doTransform(cloud, global_frame_cloud, tfm_2); global_frame_cloud.header.stamp = cloud.header.stamp;