diff --git a/CMakeLists.txt b/CMakeLists.txt index 1d06b41..10dc4bd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,6 +215,54 @@ target_link_libraries(inflation_layer yaml-cpp ) +add_library(voxel_layer SHARED + plugins/voxel_layer.cpp +) +target_link_libraries(voxel_layer + costmap_2d_new + ${Boost_LIBRARIES} + yaml-cpp +) + +add_library(critical_layer SHARED + plugins/critical_layer.cpp +) +target_link_libraries(critical_layer + costmap_2d_new + static_layer + ${Boost_LIBRARIES} + yaml-cpp +) + +add_library(directional_layer SHARED + plugins/directional_layer.cpp +) +target_link_libraries(directional_layer + costmap_2d_new + static_layer + ${Boost_LIBRARIES} + yaml-cpp +) + +add_library(preferred_layer SHARED + plugins/preferred_layer.cpp +) +target_link_libraries(preferred_layer + costmap_2d_new + static_layer + ${Boost_LIBRARIES} + yaml-cpp +) + +add_library(unpreferred_layer SHARED + plugins/unpreferred_layer.cpp +) +target_link_libraries(unpreferred_layer + costmap_2d_new + static_layer + ${Boost_LIBRARIES} + yaml-cpp +) # --- Test executables --- add_executable(test_array_parser test/array_parser_test.cpp) @@ -240,6 +288,7 @@ target_link_libraries(test_plugin PRIVATE costmap_2d_new static_layer obstacle_layer + inflation_layer ${Boost_LIBRARIES} Boost::filesystem Boost::system diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h new file mode 100644 index 0000000..e7d7b27 --- /dev/null +++ b/include/costmap_2d/costmap_2d_robot.h @@ -0,0 +1,282 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ +#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_ +#define COSTMAP_2D_COSTMAP_2D_ROS_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// class SuperValue : public XmlRpc::XmlRpcValue +// { +// public: +// void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a) +// { +// _type = TypeStruct; +// _value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a); +// } +// void setArray(XmlRpc::XmlRpcValue::ValueArray* a) +// { +// _type = TypeArray; +// _value.asArray = new std::vector(*a); +// } +// }; + +namespace costmap_2d +{ + +/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to + * topics that provide observations about obstacles in either the form + * of PointCloud or LaserScan messages. */ +class Costmap2DRobot +{ +public: + /** + * @brief Constructor for the wrapper + * @param name The name for this costmap + * @param tf A reference to a TransformListener + */ + Costmap2DRobot(const std::string &name, tf2::BufferCore& tf); + ~Costmap2DRobot(); + + /** + * @brief Subscribes to sensor topics if necessary and starts costmap + * updates, can be called to restart the costmap after calls to either + * stop() or pause() + */ + void start(); + + /** + * @brief Stops costmap updates and unsubscribes from sensor topics + */ + void stop(); + + /** + * @brief Stops the costmap from updating, but sensor data still comes in over the wire + */ + void pause(); + + /** + * @brief Resumes costmap updates + */ + void resume(); + + void updateMap(); + + /** + * @brief Reset each individual layer + */ + void resetLayers(); + + /** @brief Same as getLayeredCostmap()->isCurrent(). */ + bool isCurrent() const + { + return layered_costmap_->isCurrent(); + } + + /** + * @brief Is the costmap stopped + */ + bool isStopped() const + { + return stopped_; + } + + /** + * @brief Get the pose of the robot in the global frame of the costmap + * @param global_pose Will be set to the pose of the robot in the global frame of the costmap + * @return True if the pose was set successfully, false otherwise + */ + bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const; + + /** @brief Returns costmap name */ + inline const std::string& getName() const noexcept + { + return name_; + } + + /** @brief Returns the delay in transform (tf) data that is tolerable in seconds */ + double getTransformTolerance() const + { + return transform_tolerance_; + } + + /** @brief Return a pointer to the "master" costmap which receives updates from all the layers. + * + * Same as calling getLayeredCostmap()->getCostmap(). */ + Costmap2D* getCostmap() const + { + return layered_costmap_->getCostmap(); + } + + /** + * @brief Returns the global frame of the costmap + * @return The global frame of the costmap + */ + inline const std::string& getGlobalFrameID() const noexcept + { + return global_frame_; + } + + /** + * @brief Returns the local frame of the costmap + * @return The local frame of the costmap + */ + inline const std::string& getBaseFrameID() const noexcept + { + return robot_base_frame_; + } + LayeredCostmap* getLayeredCostmap() const + { + return layered_costmap_; + } + + /** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */ + geometry_msgs::Polygon getRobotFootprintPolygon() const + { + return costmap_2d::toPolygon(padded_footprint_); + } + + /** @brief Return the current footprint of the robot as a vector of points. + * + * This version of the footprint is padded by the footprint_padding_ + * distance, set in the rosparam "footprint_padding". + * + * The footprint initially comes from the rosparam "footprint" but + * can be overwritten by dynamic reconfigure or by messages received + * on the "footprint" topic. */ + inline const std::vector& getRobotFootprint() const noexcept + { + return padded_footprint_; + } + + /** @brief Return the current unpadded footprint of the robot as a vector of points. + * + * This is the raw version of the footprint without padding. + * + * The footprint initially comes from the rosparam "footprint" but + * can be overwritten by dynamic reconfigure or by messages received + * on the "footprint" topic. */ + inline const std::vector& getUnpaddedRobotFootprint() const noexcept + { + return unpadded_footprint_; + } + + /** + * @brief Build the oriented footprint of the robot at the robot's current pose + * @param oriented_footprint Will be filled with the points in the oriented footprint of the robot + */ + void getOrientedFootprint(std::vector& oriented_footprint) const; + + /** @brief Set the footprint of the robot to be the given set of + * points, padded by footprint_padding. + * + * Should be a convex polygon, though this is not enforced. + * + * First expands the given polygon by footprint_padding_ and then + * sets padded_footprint_ and calls + * layered_costmap_->setFootprint(). Also saves the unpadded + * footprint, which is available from + * getUnpaddedRobotFootprint(). */ + void setUnpaddedRobotFootprint(const std::vector& points); + + /** @brief Set the footprint of the robot to be the given polygon, + * padded by footprint_padding. + * + * Should be a convex polygon, though this is not enforced. + * + * First expands the given polygon by footprint_padding_ and then + * sets padded_footprint_ and calls + * layered_costmap_->setFootprint(). Also saves the unpadded + * footprint, which is available from + * getUnpaddedRobotFootprint(). */ + void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint); + +protected: + LayeredCostmap* layered_costmap_; + std::string name_; + tf2_ros::Buffer& tf_; ///< @brief Used for transforming point clouds + std::string global_frame_; ///< @brief The global frame for the costmap + std::string robot_base_frame_; ///< @brief The frame_id of the robot base + double transform_tolerance_; ///< timeout before transform errors + +private: + /** @brief Set the footprint from the new_config object. + * + * If the values of footprint and robot_radius are the same in + * new_config and old_config, nothing is changed. */ + void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, + const costmap_2d::Costmap2DConfig &old_config); + + void loadOldParameters(ros::NodeHandle& nh); + void warnForOldParameters(ros::NodeHandle& nh); + void checkOldParam(ros::NodeHandle& nh, const std::string ¶m_name); + void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh); + // void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level); + void movementCB(const ros::TimerEvent &event); + void mapUpdateLoop(double frequency); + bool map_update_thread_shutdown_; + bool stop_updates_, initialized_, stopped_; + boost::thread* map_update_thread_; ///< @brief A thread for updating the map + // ros::Time last_publish_; + // ros::Duration publish_cycle; + // pluginlib::ClassLoader plugin_loader_; + // Costmap2DPublisher* publisher_; + // dynamic_reconfigure::Server *dsrv_; + + boost::recursive_mutex configuration_mutex_; + + // ros::Subscriber footprint_sub_; + // ros::Publisher footprint_pub_; + std::vector unpadded_footprint_; + std::vector padded_footprint_; + float footprint_padding_; + // costmap_2d::Costmap2DConfig old_config_; +}; +// class Costmap2DRobot +} // namespace costmap_2d + +#endif // COSTMAP_2D_COSTMAP_2D_ROS_H diff --git a/include/costmap_2d/critical_layer.h b/include/costmap_2d/critical_layer.h index eb4e3ee..86e354f 100644 --- a/include/costmap_2d/critical_layer.h +++ b/include/costmap_2d/critical_layer.h @@ -11,8 +11,8 @@ public: CriticalLayer(); virtual ~CriticalLayer(); private: - unsigned char interpretValue(unsigned char value); - void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + unsigned char interpretValue(unsigned char value) override; + void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override; }; } diff --git a/include/costmap_2d/data_convert.h b/include/costmap_2d/data_convert.h index 40ad66b..c9cfcae 100644 --- a/include/costmap_2d/data_convert.h +++ b/include/costmap_2d/data_convert.h @@ -2,12 +2,60 @@ #define DATA_CONVERT_H #include +#include +#include +#include #include #include #include +#include namespace costmap_2d { + template + bool isEqual(const T& a, const T& b, double eps = 1e-5) + { + return std::fabs(a - b) < eps; + } + + inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b) + { + return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w); + } + + inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b) + { + return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z); + } + + inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b) + { + return (a.position == b.position) && (a.orientation == b.orientation); + } + + inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) + { + return (a.header.frame_id == b.header.frame_id) && + (a.pose == b.pose); + } + + inline double getYaw(const geometry_msgs::Quaternion& q) + { + double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); + double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); + return std::atan2(siny_cosp, cosy_cosp); + } + + inline geometry_msgs::Quaternion getQuaternion(const double& yaw) + { + geometry_msgs::Quaternion q; + q.x = 0.0; + q.y = 0.0; + q.z = std::sin(yaw / 2.0); + q.w = std::cos(yaw / 2.0); + return q; + } + robot::Time convertTime(tf2::Time time) { robot::Time time_tmp; @@ -24,6 +72,16 @@ namespace costmap_2d return time_tmp; } + tf2::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) + { + tf2::Quaternion out(q.x,q.y,q.z,q.w); + return out; + } + + geometry_msgs::Quaternion convertQuaternion(const tf2::Quaternion& q) + { + return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); + } tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg) { diff --git a/include/costmap_2d/directional_layer.h b/include/costmap_2d/directional_layer.h index c3628b3..452c946 100644 --- a/include/costmap_2d/directional_layer.h +++ b/include/costmap_2d/directional_layer.h @@ -2,8 +2,7 @@ #define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_ #include -// #include -// #include +#include namespace costmap_2d { @@ -12,14 +11,14 @@ namespace costmap_2d public: DirectionalLayer(); virtual ~DirectionalLayer(); - // bool laneFilter(const std::vector plan); + bool laneFilter(const std::vector plan); void resetMap(); private: - // void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map); - // bool laneFilter(std::vector> new_map, const nav_msgs::Path path); + void incomingMap(const nav_msgs::OccupancyGrid &new_map); + bool laneFilter(std::vector> new_map, const nav_msgs::Path path); unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot); - // void laneFilter(std::vector x, std::vector y, std::vector yaw_robot); + void laneFilter(std::vector x, std::vector y, std::vector yaw_robot); void laneFilter(std::vector> x, std::vector> y, std::vector yaw_robot); bool inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance); void convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th); @@ -30,7 +29,6 @@ namespace costmap_2d double distance_skip_ = 1.0; // ros::Publisher lane_mask_pub_; nav_msgs::OccupancyGrid new_map_; - // tf::TransformListener listener_; }; } diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index 01cd37a..e1abc8e 100644 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -76,38 +76,6 @@ public: virtual void deactivate(); virtual void reset(); - // /** - // * @brief A callback to handle buffering LaserScan messages - // * @param message The message returned from a message notifier - // * @param buffer A pointer to the observation buffer to update - // */ - // void laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, - // const boost::shared_ptr& buffer); - - // /** - // * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. - // * @param message The message returned from a message notifier - // * @param buffer A pointer to the observation buffer to update - // */ - // void laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& message, - // const boost::shared_ptr& buffer); - - // /** - // * @brief A callback to handle buffering PointCloud messages - // * @param message The message returned from a message notifier - // * @param buffer A pointer to the observation buffer to update - // */ - // void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, - // const boost::shared_ptr& buffer); - - // /** - // * @brief A callback to handle buffering PointCloud2 messages - // * @param message The message returned from a message notifier - // * @param buffer A pointer to the observation buffer to update - // */ - // void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, - // const boost::shared_ptr& buffer); - // for testing purposes void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing); void clearStaticObservations(bool marking, bool clearing); @@ -165,8 +133,6 @@ protected: laser_geometry::LaserProjection projector_; ///< @brief Used to project laser scans into point clouds - // std::vector > observation_subscribers_; ///< @brief Used for the observation message filters - // std::vector > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles @@ -183,6 +149,8 @@ private: bool getParams(); }; +using PluginObstacleLayerPtr = std::shared_ptr; + } // namespace costmap_2d #endif // COSTMAP_2D_OBSTACLE_LAYER_H_ diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index 87c55c9..ff3640f 100644 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -27,7 +27,7 @@ public: virtual void matchSize(); protected: - void handleImpl(const void* data, + void handleImpl(const void* data, const std::type_info& type, const std::string& topic) override; void incomingMap(const nav_msgs::OccupancyGrid& new_map); diff --git a/include/costmap_2d/voxel_grid.h b/include/costmap_2d/voxel_grid.h deleted file mode 100644 index ce838b9..0000000 --- a/include/costmap_2d/voxel_grid.h +++ /dev/null @@ -1,30 +0,0 @@ -// Header header -// uint32_t[] data -// geometry_msgs/Point32 origin -// geometry_msgs/Vector3 resolutions -// uint32_t size_x -// uint32_t size_y -// uint32_t size_z -#ifndef COSTMAP_2D_VOXEL_GRID_H_ -#define COSTMAP_2D_VOXEL_GRID_H_ - -#include -#include -#include - -namespace costmap_2d -{ - struct VoxelGrid - { - std_msgs::Header header; - std::vector data; - geometry_msgs::Point32 origin; - geometry_msgs::Vector3 resolutions; - uint32_t size_x; - uint32_t size_y; - uint32_t size_z; - }; -} - - -#endif // COSTMAP_2D_VOXEL_GRID_H_ \ No newline at end of file diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h index f7ec71e..e12c436 100644 --- a/include/costmap_2d/voxel_layer.h +++ b/include/costmap_2d/voxel_layer.h @@ -42,7 +42,7 @@ #include #include #include -#include +// #include #include #include // #include diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp index ab6e74c..22e1554 100644 --- a/plugins/critical_layer.cpp +++ b/plugins/critical_layer.cpp @@ -13,6 +13,7 @@ CriticalLayer::~CriticalLayer(){} unsigned char CriticalLayer::interpretValue(unsigned char value) { + // printf("TEST PLUGIN CRITICAL\n"); // check if the static value is above the unknown or lethal thresholds if(value >= *this->threshold_) return CRITICAL_SPACE; @@ -22,12 +23,13 @@ unsigned char CriticalLayer::interpretValue(unsigned char value) void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { + // printf("TEST PLUGIN CRITICAL\n"); if (!map_received_) return; // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + StaticLayer::CostmapLayer::updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); else updateWithMax(master_grid, min_i, min_j, max_i, max_j); @@ -40,4 +42,6 @@ static PluginStaticLayerPtr create_critical_plugin() { // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) BOOST_DLL_ALIAS(create_critical_plugin, create_plugin_static_layer) + + } \ No newline at end of file diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index eaf66e0..6230b0b 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -1,11 +1,9 @@ #include -// #include -// #include -// #include -// #include -// #include +#include +#include +#include -// PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer) +#include #define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) @@ -15,398 +13,426 @@ using costmap_2d::NO_INFORMATION; namespace costmap_2d { - // DirectionalLayer::DirectionalLayer() - // { - // ros::NodeHandle nh("~/" + name_); - // lane_mask_pub_ = nh.advertise("/direction_zones/lanes", 1); - // } - // DirectionalLayer::~DirectionalLayer() {} + DirectionalLayer::DirectionalLayer() + { + // ros::NodeHandle nh("~/" + name_); + // lane_mask_pub_ = nh.advertise("/direction_zones/lanes", 1); + } + DirectionalLayer::~DirectionalLayer() {} - // bool DirectionalLayer::laneFilter(const std::vector plan) - // { - // if (directional_areas_.empty()) - // throw "Has no map data"; - // nav_msgs::Path path; - // path.header.stamp = ros::Time::now(); - // path.header.frame_id = map_frame_; - // path.poses = plan; - // return this->laneFilter(directional_areas_, path); - // } + bool DirectionalLayer::laneFilter(const std::vector plan) + { + if (directional_areas_.empty()) + throw "Has no map data"; + nav_msgs::Path path; + path.header.stamp = robot::Time::now(); + path.header.frame_id = map_frame_; + path.poses = plan; + return this->laneFilter(directional_areas_, path); + } - // void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map) - // { - // unsigned int size_x = new_map->info.width, size_y = new_map->info.height; + void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map) + { + if(!map_shutdown_) + { + unsigned int size_x = new_map.info.width, size_y = new_map.info.height; - // ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution); + printf("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution); - // // resize costmap if size, resolution or origin do not match - // Costmap2D *master = layered_costmap_->getCostmap(); - // if (!layered_costmap_->isRolling() && - // (master->getSizeInCellsX() != size_x / 2 || - // master->getSizeInCellsY() != size_y / 2 || - // master->getResolution() != new_map->info.resolution || - // master->getOriginX() != new_map->info.origin.position.x || - // master->getOriginY() != new_map->info.origin.position.y)) - // { - // // Update the size of the layered costmap (and all layers, including this one) - // ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution); - // layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, new_map->info.origin.position.x, - // new_map->info.origin.position.y, - // true /* set size_locked to true, prevents reconfigureCb from overriding map size*/); - // } - // else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 || - // resolution_ != new_map->info.resolution || - // origin_x_ != new_map->info.origin.position.x || - // origin_y_ != new_map->info.origin.position.y) - // { - // // only update the size of the costmap stored locally in this layer - // ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x / 2, size_y / 2, new_map->info.resolution); - // resizeMap(size_x / 2, size_y / 2, new_map->info.resolution, - // new_map->info.origin.position.x, new_map->info.origin.position.y); - // } - // unsigned char values[4]; - // directional_areas_.clear(); - // directional_areas_.resize(size_x / 2 * size_y / 2); - // // initialize the costmap with static data - // for (unsigned int iy = 0; iy < size_y; iy += 2) - // { - // for (unsigned int ix = 0; ix < size_x; ix += 2) - // { - // values[0] = new_map->data[MAP_IDX(size_x, ix, iy)]; - // values[1] = new_map->data[MAP_IDX(size_x, ix + 1, iy)]; - // values[2] = new_map->data[MAP_IDX(size_x, ix, iy + 1)]; - // values[3] = new_map->data[MAP_IDX(size_x, ix + 1, iy + 1)]; - // uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u); - // int index = getIndex(ix / 2, iy / 2); - // directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff); - // directional_areas_[index][1] = (uint16_t)(color_avg >> 12u); - // costmap_[index] = costmap_2d::NO_INFORMATION; - // } - // } - // map_frame_ = new_map->header.frame_id; + // resize costmap if size, resolution or origin do not match + Costmap2D *master = layered_costmap_->getCostmap(); + if (!layered_costmap_->isRolling() && + (master->getSizeInCellsX() != size_x / 2 || + master->getSizeInCellsY() != size_y / 2 || + master->getResolution() != new_map.info.resolution || + master->getOriginX() != new_map.info.origin.position.x || + master->getOriginY() != new_map.info.origin.position.y)) + { + // Update the size of the layered costmap (and all layers, including this one) + printf("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution); + layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map.info.resolution, new_map.info.origin.position.x, + new_map.info.origin.position.y, + true /* set size_locked to true, prevents reconfigureCb from overriding map size*/); + } + else if (size_x_ != size_x / 2 || size_y_ != size_y / 2 || + resolution_ != new_map.info.resolution || + origin_x_ != new_map.info.origin.position.x || + origin_y_ != new_map.info.origin.position.y) + { + // only update the size of the costmap stored locally in this layer + printf("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution); + resizeMap(size_x / 2, size_y / 2, new_map.info.resolution, + new_map.info.origin.position.x, new_map.info.origin.position.y); + } + unsigned char values[4]; + directional_areas_.clear(); + directional_areas_.resize(size_x / 2 * size_y / 2); + // initialize the costmap with static data + for (unsigned int iy = 0; iy < size_y; iy += 2) + { + for (unsigned int ix = 0; ix < size_x; ix += 2) + { + values[0] = new_map.data[MAP_IDX(size_x, ix, iy)]; + values[1] = new_map.data[MAP_IDX(size_x, ix + 1, iy)]; + values[2] = new_map.data[MAP_IDX(size_x, ix, iy + 1)]; + values[3] = new_map.data[MAP_IDX(size_x, ix + 1, iy + 1)]; + uint32_t color_avg = (uint32_t)(values[0] | values[1] << 8u | values[2] << 16u | values[3] << 24u); + int index = getIndex(ix / 2, iy / 2); + directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff); + directional_areas_[index][1] = (uint16_t)(color_avg >> 12u); + costmap_[index] = costmap_2d::NO_INFORMATION; + } + } + map_frame_ = new_map.header.frame_id; - // // we have a new map, update full size of map - // x_ = y_ = 0; - // width_ = size_x_; - // height_ = size_y_; - // map_received_ = true; - // has_updated_data_ = true; + // we have a new map, update full size of map + x_ = y_ = 0; + width_ = size_x_; + height_ = size_y_; + map_received_ = true; + has_updated_data_ = true; - // new_map_.header = new_map->header; - // new_map_.header.stamp = ros::Time::now(); - // new_map_.info = new_map->info; - // new_map_.info.width = width_; - // new_map_.info.height = height_; + new_map_.header = new_map.header; + new_map_.header.stamp = robot::Time::now(); + new_map_.info = new_map.info; + new_map_.info.width = width_; + new_map_.info.height = height_; + } + else + { + printf("Stop receive new map!\n"); + } + // shutdown the map subscrber if firt_map_only_ flag is on + if (first_map_only_) + { + printf("Shutting down the map subscriber. first_map_only flag is on\n"); + map_shutdown_ = true; + // map_sub_.shutdown(); + } + } - // // shutdown the map subscrber if firt_map_only_ flag is on - // if (first_map_only_) - // { - // ROS_INFO("Shutting down the map subscriber. first_map_only flag is on"); - // map_sub_.shutdown(); - // } - // } + bool DirectionalLayer::laneFilter(std::vector> new_map, const nav_msgs::Path path) + { + boost::unique_lock lock(*layered_costmap_->getCostmap()->getMutex()); + if (new_map.empty()) + return false; - // bool DirectionalLayer::laneFilter(std::vector> new_map, const nav_msgs::Path path) - // { - // boost::unique_lock lock(*layered_costmap_->getCostmap()->getMutex()); - // if (new_map.empty()) - // return false; + std::vector> X_List, Y_List; + std::vector Yaw_list; - // std::vector> X_List, Y_List; - // std::vector Yaw_list; - - // const geometry_msgs::PoseStamped &e = path.poses.back(); + const geometry_msgs::PoseStamped &e = path.poses.back(); - // bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_); - // if(!get_success) return false; - // for (auto it = path.poses.begin(); it != path.poses.end(); ++it) - // { - // const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator - // unsigned int mx, my; - // if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my)) - // { - // ROS_ERROR("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y); - // return false; - // } - // // Convert to yaw - // tf::Quaternion quaternion; - // tf::quaternionMsgToTF(p.pose.orientation, quaternion); - // double theta = tf::getYaw(quaternion); - // unsigned char value = laneFilter(mx, my, theta); - // if (get_success) - // { - // if ( - // (!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) & - // !inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_)) - // || p == path.poses.back() - // ) - // { - // if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE) - // { - // std::pair x_val(mx, p.pose.position.x); - // std::pair y_val(my, p.pose.position.y); - // X_List.push_back(x_val); - // Y_List.push_back(y_val); - // Yaw_list.push_back(theta); - // // costmap_[getIndex(mx, my)] = value; - // } - // } - // } - // } - // if (!Yaw_list.empty()) - // { - // laneFilter(X_List, Y_List, Yaw_list); - // nav_msgs::OccupancyGrid lanes; - // convertToMap(costmap_, lanes, 0.65, 0.196); - // lane_mask_pub_.publish(lanes); - // return false; - // } - // return true; + bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_); + if(!get_success) return false; + for (auto it = path.poses.begin(); it != path.poses.end(); ++it) + { + const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator + unsigned int mx, my; + if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my)) + { + printf("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y); + return false; + } + // Convert to yaw + tf2::Quaternion quaternion = convertQuaternion(p.pose.orientation); + double theta = tf2::getYaw(quaternion); + unsigned char value = laneFilter(mx, my, theta); + if (get_success) + { + if ( + (!inSkipErea(pose_x_, pose_y_, p.pose.position.x, p.pose.position.y, distance_skip_) & + !inSkipErea(p.pose.position.x, p.pose.position.y, e.pose.position.x, e.pose.position.y, distance_skip_)) + || p == path.poses.back() + ) + { + if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE) + { + std::pair x_val(mx, p.pose.position.x); + std::pair y_val(my, p.pose.position.y); + X_List.push_back(x_val); + Y_List.push_back(y_val); + Yaw_list.push_back(theta); + // costmap_[getIndex(mx, my)] = value; + } + } + } + } + if (!Yaw_list.empty()) + { + laneFilter(X_List, Y_List, Yaw_list); + nav_msgs::OccupancyGrid lanes; + convertToMap(costmap_, lanes, 0.65, 0.196); + + ////////////////////////////////// + ////////////////////////////////// + /////////THAY THẾ PUBLISH//////// + // lane_mask_pub_.publish(lanes); + ////////////////////////////////// + ////////////////////////////////// + ////////////////////////////////// + + + return false; + } + return true; - // } + } - // void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th) - // { - // dir_map.header = new_map_.header; - // dir_map.header.stamp = ros::Time::now(); - // dir_map.info = new_map_.info; - // int size_costmap = new_map_.info.width * new_map_.info.height; - // dir_map.data.resize(size_costmap); - // for (int i = 0; i < size_costmap; i++) - // { - // int8_t value; - // if (costmap[i] == costmap_2d::NO_INFORMATION) - // { - // value = -1; - // } - // else - // { - // double occ = (costmap[i]) / 255.0; - // if (occ > occ_th) - // value = +100; - // else if (occ < free_th) - // value = 0; - // else - // { - // double ratio = (occ - free_th) / (occ_th - free_th); - // value = 1 + 98 * ratio; - // } - // } - // dir_map.data[i] = value; - // } - // } + void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th) + { + dir_map.header = new_map_.header; + dir_map.header.stamp = robot::Time::now(); + dir_map.info = new_map_.info; + int size_costmap = new_map_.info.width * new_map_.info.height; + dir_map.data.resize(size_costmap); + for (int i = 0; i < size_costmap; i++) + { + int8_t value; + if (costmap[i] == costmap_2d::NO_INFORMATION) + { + value = -1; + } + else + { + double occ = (costmap[i]) / 255.0; + if (occ > occ_th) + value = +100; + else if (occ < free_th) + value = 0; + else + { + double ratio = (occ - free_th) / (occ_th - free_th); + value = 1 + 98 * ratio; + } + } + dir_map.data[i] = value; + } + } - // unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot) - // { - // double yaw_lane; - // unsigned char cost = costmap_2d::NO_INFORMATION; + unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot) + { + double yaw_lane; + unsigned char cost = costmap_2d::NO_INFORMATION; - // int index = getIndex(x, y); - // for (auto &lane : directional_areas_[index]) - // { - // if (lane > 359) - // { - // cost = std::min(cost, costmap_2d::NO_INFORMATION); - // } - // else - // { - // double yaw_lane = (double)lane / 180 * M_PI; - // if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0) - // cost = std::min(cost, costmap_2d::FREE_SPACE); - // else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4) - // cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE); - // else - // cost = std::min(cost, costmap_2d::NO_INFORMATION); - // } - // } - // return cost; - // } + int index = getIndex(x, y); + for (auto &lane : directional_areas_[index]) + { + if (lane > 359) + { + cost = std::min(cost, costmap_2d::NO_INFORMATION); + } + else + { + double yaw_lane = (double)lane / 180 * M_PI; + if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0) + cost = std::min(cost, costmap_2d::FREE_SPACE); + else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4) + cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE); + else + cost = std::min(cost, costmap_2d::NO_INFORMATION); + } + } + return cost; + } - // void DirectionalLayer::laneFilter(std::vector> x, - // std::vector> y, - // std::vector yaw_robot) - // { - // if(x.empty() || y.empty() || yaw_robot.empty()) - // return; + void DirectionalLayer::laneFilter(std::vector> x, + std::vector> y, + std::vector yaw_robot) + { + if(x.empty() || y.empty() || yaw_robot.empty()) + return; - // unsigned int x_min, y_min, x_max, y_max; - // double x_min_w, y_min_w, x_max_w, y_max_w; + unsigned int x_min, y_min, x_max, y_max; + double x_min_w, y_min_w, x_max_w, y_max_w; - // x_min = x.front().first; y_min = y.front().first; - // x_max = x.front().first; y_max = y.front().first; - // x_min_w = x.front().second; y_min_w = y.front().second; - // x_max_w = x.front().second; y_max_w = y.front().second; - // for (int i = 1; i < yaw_robot.size(); i++) - // { - // if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_)) - // continue; - // x_min = std::min(x_min, x[i].first); - // y_min = std::min(y_min, y[i].first); - // x_max = std::max(x_max, x[i].first); - // y_max = std::max(y_max, y[i].first); + x_min = x.front().first; y_min = y.front().first; + x_max = x.front().first; y_max = y.front().first; + x_min_w = x.front().second; y_min_w = y.front().second; + x_max_w = x.front().second; y_max_w = y.front().second; + for (int i = 1; i < yaw_robot.size(); i++) + { + if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_)) + continue; + x_min = std::min(x_min, x[i].first); + y_min = std::min(y_min, y[i].first); + x_max = std::max(x_max, x[i].first); + y_max = std::max(y_max, y[i].first); - // x_min_w = std::min(x_min_w, x[i].second); - // y_min_w = std::min(y_min_w, y[i].second); - // x_max_w = std::max(x_max_w, x[i].second); - // y_max_w = std::max(y_max_w, y[i].second); + x_min_w = std::min(x_min_w, x[i].second); + y_min_w = std::min(y_min_w, y[i].second); + x_max_w = std::max(x_max_w, x[i].second); + y_max_w = std::max(y_max_w, y[i].second); - // } - // // ROS_INFO("%d %d %d %d", x_min, y_min, x_max, y_max); - // // ROS_INFO("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w); - // for (int i = 0; i < yaw_robot.size(); i++) - // { - // if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_)) - // continue; + } + // printf("%d %d %d %d", x_min, y_min, x_max, y_max); + // printf("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w); + for (int i = 0; i < yaw_robot.size(); i++) + { + if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_)) + continue; - // double yaw_rad = (double)yaw_robot[i] / 180 * M_PI; - // if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad))) - // { - // int x_ = x[i].first; - // // Dưới lên trên - // for (int j = y[i].first; j <= y_max; j++) - // { - // int y_ = j; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // // Trên xuống dưới - // for (int k = y[i].first; k >= y_min; k--) - // { - // int y_ = k; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } + double yaw_rad = (double)yaw_robot[i] / 180 * M_PI; + if (fabs(cos(yaw_rad)) > fabs(sin(yaw_rad))) + { + int x_ = x[i].first; + // Dưới lên trên + for (int j = y[i].first; j <= y_max; j++) + { + int y_ = j; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + // Trên xuống dưới + for (int k = y[i].first; k >= y_min; k--) + { + int y_ = k; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } - // int y_ = y[i].first; - // // Phải qua trái - // for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++) - // { - // int x_ = j; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // // Trái qua phải - // for (int k = x[i].first; k >= 0; k--) - // { - // int x_ = k; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // } - // else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad))) - // { - // int y_ = y[i].first; - // // Phải qua trái - // for (int j = x[i].first; j <= x_max; j++) - // { - // int x_ = j; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // // Trái qua phải - // for (int k = x[i].first; k >= x_min; k--) - // { - // int x_ = k; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } + int y_ = y[i].first; + // Phải qua trái + for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++) + { + int x_ = j; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + // Trái qua phải + for (int k = x[i].first; k >= 0; k--) + { + int x_ = k; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + } + else if(fabs(cos(yaw_rad)) < fabs(sin(yaw_rad))) + { + int y_ = y[i].first; + // Phải qua trái + for (int j = x[i].first; j <= x_max; j++) + { + int x_ = j; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + // Trái qua phải + for (int k = x[i].first; k >= x_min; k--) + { + int x_ = k; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } - // int x_ = x[i].first; - // // Dưới lên trên - // for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++) - // { - // int y_ = j; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // // Trên xuống dưới - // for (int k = y[i].first; k >= 0; k--) - // { - // int y_ = k; - // if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) - // continue; - // unsigned char value = laneFilter(x_, y_, yaw_robot[i]); - // if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) - // costmap_[getIndex(x_, y_)] = value; - // else - // break; - // } - // } - // } - // } + int x_ = x[i].first; + // Dưới lên trên + for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++) + { + int y_ = j; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + // Trên xuống dưới + for (int k = y[i].first; k >= 0; k--) + { + int y_ = k; + if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE) + continue; + unsigned char value = laneFilter(x_, y_, yaw_robot[i]); + if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION) + costmap_[getIndex(x_, y_)] = value; + else + break; + } + } + } + } - // void DirectionalLayer::resetMap() - // { - // unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX(); - // unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY(); - // for (unsigned int iy = 0; iy < size_y; iy++) - // { - // for (unsigned int ix = 0; ix < size_x; ix++) - // { - // int index = getIndex(ix, iy); - // costmap_[index] = costmap_2d::NO_INFORMATION; - // } - // } - // } + void DirectionalLayer::resetMap() + { + unsigned int size_x = layered_costmap_->getCostmap()->getSizeInCellsX(); + unsigned int size_y = layered_costmap_->getCostmap()->getSizeInCellsY(); + for (unsigned int iy = 0; iy < size_y; iy++) + { + for (unsigned int ix = 0; ix < size_x; ix++) + { + int index = getIndex(ix, iy); + costmap_[index] = costmap_2d::NO_INFORMATION; + } + } + } - // bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw) - // { - // tf::StampedTransform transform; - // try - // { - // listener_.lookupTransform(map_frame_, base_frame_id_, ros::Time(0), transform); - // } - // catch (tf::TransformException &ex) - // { - // ROS_ERROR("%s", ex.what()); - // return false; - // } - // x = transform.getOrigin().x(); - // y = transform.getOrigin().y(); - // // Extract the rotation as a quaternion from the transform - // tf::Quaternion rotation = transform.getRotation(); - // // Convert the quaternion to a yaw angle (in radians) - // yaw = tf::getYaw(rotation); - // return true; - // } + bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw) + { + tf2::TransformStampedMsg transformMsg; + try + { + transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf2::Time()); + + // listener_.lookupTransform(map_frame_, base_frame_id_, tf2::Time(), transform); + } + catch (tf2::TransformException &ex) + { + printf("%s\n", ex.what()); + return false; + } + // Copy map data given proper transformations + tf2::Transform tf2_transform; + tf2_transform = convertToTf2Transform(transformMsg.transform); + x = tf2_transform.getOrigin().x(); + y = tf2_transform.getOrigin().y(); + // Extract the rotation as a quaternion from the transform + tf2::Quaternion rotation = tf2_transform.getRotation(); + // Convert the quaternion to a yaw angle (in radians) + yaw = tf2::getYaw(rotation); + return true; + } - // bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance) - // { - // return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance; - // } + bool DirectionalLayer::inSkipErea(double start_x, double start_y, double end_x, double end_y, double skip_distance) + { + return fabs(hypot(start_x - end_x, start_y - end_y)) <= skip_distance; + } + + // Export factory function + static PluginStaticLayerPtr create_directional_plugin() { + return std::make_shared(); + } + + // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) + BOOST_DLL_ALIAS(create_directional_plugin, create_plugin_static_layer) } \ No newline at end of file diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 8fb12ef..a7a48ae 100644 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -53,7 +53,7 @@ void InflationLayer::onInitialize() bool InflationLayer::getParams() { try { - YAML::Node config = YAML::LoadFile("../cfg/config.yaml"); + YAML::Node config = YAML::LoadFile("/home/duongtd/robotics_core/costmap_2d/config/config.yaml"); YAML::Node layer = config["inflation_layer"]; double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0); double inflation_radius = loadParam(layer, "inflation_radius", 0.55); @@ -77,17 +77,6 @@ bool InflationLayer::getParams() } -// void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) -// { -// setInflationParameters(config.inflation_radius, config.cost_scaling_factor); - -// if (enabled_ != config.enabled || inflate_unknown_ != config.inflate_unknown) { -// enabled_ = config.enabled; -// inflate_unknown_ = config.inflate_unknown; -// need_reinflation_ = true; -// } -// } - void InflationLayer::matchSize() { boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); @@ -146,7 +135,7 @@ void InflationLayer::onFootprintChanged() need_reinflation_ = true; printf("InflationLayer::onFootprintChanged(): num footprint points: %lu," - " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", + " inscribed_radius_ = %.3f, inflation_radius_ = %.3f\n", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); } diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index ee03361..306ea49 100644 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -510,10 +510,10 @@ void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double void ObstacleLayer::reset() { -// deactivate(); -// resetMaps(); -// current_ = true; -// activate(); + deactivate(); + resetMaps(); + current_ = true; + activate(); } // Export factory function diff --git a/plugins/preferred_layer.cpp b/plugins/preferred_layer.cpp index 17afe9c..dec8347 100644 --- a/plugins/preferred_layer.cpp +++ b/plugins/preferred_layer.cpp @@ -14,6 +14,7 @@ PreferredLayer::~PreferredLayer(){} unsigned char PreferredLayer::interpretValue(unsigned char value) { + printf("TEST PLUGIN !!!\n"); // check if the static value is above the unknown or lethal thresholds if(value == 0) return NO_INFORMATION; else if (value >= *this->threshold_) @@ -24,11 +25,11 @@ unsigned char PreferredLayer::interpretValue(unsigned char value) } // Export factory function -static PluginPtr create_preferred_plugin() { +static PluginStaticLayerPtr create_preferred_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin) +BOOST_DLL_ALIAS(create_preferred_plugin, create_plugin_static_layer) } \ No newline at end of file diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index d825bda..c12aee4 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -104,8 +104,8 @@ unsigned char StaticLayer::interpretValue(unsigned char value) } void StaticLayer::handleImpl(const void* data, - const std::type_info& type, - const std::string& topic) + const std::type_info& type, + const std::string& topic) { if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") { incomingMap(*static_cast(data)); diff --git a/plugins/unpreferred_layer.cpp b/plugins/unpreferred_layer.cpp index 5fceb29..484e8f4 100644 --- a/plugins/unpreferred_layer.cpp +++ b/plugins/unpreferred_layer.cpp @@ -23,11 +23,11 @@ unsigned char UnPreferredLayer::interpretValue(unsigned char value) } // Export factory function -static PluginPtr create_unpreferred_plugin() { +static PluginStaticLayerPtr create_unpreferred_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin) +BOOST_DLL_ALIAS(create_unpreferred_plugin, create_plugin_static_layer) } \ No newline at end of file diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index d56b9bf..6afe05b 100644 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -53,17 +53,17 @@ using costmap_2d::Observation; namespace costmap_2d { -// void VoxelLayer::onInitialize() -// { -// ObstacleLayer::onInitialize(); -// ros::NodeHandle private_nh("~/" + name_); +void VoxelLayer::onInitialize() +{ + ObstacleLayer::onInitialize(); + // ros::NodeHandle private_nh("~/" + name_); // private_nh.param("publish_voxel_map", publish_voxel_, false); -// if (publish_voxel_) -// voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1); + // if (publish_voxel_) + // voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1); -// clearing_endpoints_pub_ = private_nh.advertise("clearing_endpoints", 1); -// } + // clearing_endpoints_pub_ = private_nh.advertise("clearing_endpoints", 1); +} // void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh) // { @@ -90,12 +90,12 @@ VoxelLayer::~VoxelLayer() // matchSize(); // } -// void VoxelLayer::matchSize() -// { -// ObstacleLayer::matchSize(); -// voxel_grid_.resize(size_x_, size_y_, size_z_); -// ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); -// } +void VoxelLayer::matchSize() +{ + ObstacleLayer::matchSize(); + // voxel_grid_.resize(size_x_, size_y_, size_z_); + // ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_); +} // void VoxelLayer::reset() // { @@ -445,11 +445,11 @@ VoxelLayer::~VoxelLayer() // } // Export factory function -static PluginPtr create_voxel_plugin() { +static PluginObstacleLayerPtr create_voxel_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) -BOOST_DLL_ALIAS(create_voxel_plugin, create_plugin) +BOOST_DLL_ALIAS(create_voxel_plugin, create_obstacle_plugin) } // namespace costmap_2d diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 7fbac41..8b44b3a 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -7,31 +7,190 @@ #include "nav_msgs/OccupancyGrid.h" #include #include +#include +#include +#include using namespace costmap_2d; int main(int argc, char* argv[]) { - auto creator = boost::dll::import_alias( - "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations + // auto creator = boost::dll::import_alias( + // "./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations + // ); + + // costmap_2d::PluginCostmapLayerPtr plugin = creator(); + // std::cout << "Plugin created successfully" << std::endl; + + // std::string global_frame = "map"; + // bool rolling_window = true; + // bool track_unknown = true; + // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); + + // tf2::BufferCore tf_buffer; + // // tf2::Duration cache_time(10.0); + // // tf2::BufferCore tf_buffer(cache_time); + + // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer); + + // std::cout << "Plugin initialized" << std::endl; + + // // plugin->activate(); + // nav_msgs::OccupancyGrid grid; + + // grid.info.resolution = 0.05f; + // grid.info.width = 2; + // grid.info.height = 2; + + // grid.data.resize(grid.info.width * grid.info.height, -1); + // grid.data[0] = 0; + // grid.data[1] = 100; + // grid.data[2] = 10; + // grid.data[3] = 0; + // plugin->dataCallBack(grid, "map"); + + // map_msgs::OccupancyGridUpdate update; + + // update.x = 1; + // update.y = 1; + // update.width = 2; + // update.height = 2; + + // update.data.resize(update.width * update.height, -1); + // update.data[0] = 0; + // update.data[1] = 100; + // update.data[2] = 10; + // update.data[3] = 0; + // plugin->dataCallBack(update, "map_update"); + + + // creator = boost::dll::import_alias( + // "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations + // ); + + // plugin = creator(); + // std::cout << "Plugin created successfully" << std::endl; + + // plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer); + + // sensor_msgs::LaserScan scan; + + // // --- Header --- + // scan.header.seq = 1; + // scan.header.stamp = robot::Time::now(); // nếu bạn chưa có Time class, gán tạm số + // scan.header.frame_id = "laser_frame"; + + // // --- Cấu hình góc --- + // scan.angle_min = -M_PI; // -180 độ + // scan.angle_max = M_PI; // +180 độ + // scan.angle_increment = M_PI / 180; // 1 độ mỗi tia + + // // --- Cấu hình thời gian --- + // scan.time_increment = 0.0001f; // mỗi tia cách nhau 0.1 ms + // scan.scan_time = 0.04f; // tổng thời gian quét 40 ms (~25Hz) + + // // --- Giới hạn đo --- + // scan.range_min = 0.1f; + // scan.range_max = 10.0f; + + // // --- Tạo dữ liệu --- + // int num_readings = static_cast((scan.angle_max - scan.angle_min) / scan.angle_increment); + // scan.ranges.resize(num_readings); + // scan.intensities.resize(num_readings); + + // for (int i = 0; i < num_readings; ++i) + // { + // float angle = scan.angle_min + i * scan.angle_increment; + + // // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc + // scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle); + + // // Giả lập cường độ phản xạ + // scan.intensities[i] = 100.0f + 20.0f * std::cos(angle); + // } + + // // --- In thử 10 giá trị đầu --- + // // for (int i = 0; i < 10; ++i) + // // { + // // std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment) + // // << " rad | Range " << scan.ranges[i] + // // << " m | Intensity " << scan.intensities[i] + // // << std::endl; + // // } + // plugin->deactivate(); + // plugin->dataCallBack(scan, "laser_valid"); + + // sensor_msgs::PointCloud cloud; + + // // 2. Thiết lập header + // cloud.header.stamp.sec = 1234567890; // hoặc ros::Time::now().sec + // cloud.header.stamp.nsec = 0; + // cloud.header.frame_id = "laser_frame"; + + // // 3. Thêm một vài điểm + // geometry_msgs::Point32 pt1; + // pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f; + + // geometry_msgs::Point32 pt2; + // pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f; + + // geometry_msgs::Point32 pt3; + // pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f; + + // cloud.points.push_back(pt1); + // cloud.points.push_back(pt2); + // cloud.points.push_back(pt3); + + // // 4. Thêm dữ liệu channels (tùy chọn) + // cloud.channels.resize(1); + // cloud.channels[0].name = "intensity"; + // cloud.channels[0].values.push_back(0.5f); + // cloud.channels[0].values.push_back(1.0f); + // cloud.channels[0].values.push_back(0.8f); + + // plugin->dataCallBack(cloud, "pcl_cb"); + + + + + + + // // std::cout << "Plugin activated successfully" << std::endl; + // auto creator = boost::dll::import_alias( + // "./costmap_2d/libinflation_layer.so", "create_plugin_layer", boost::dll::load_mode::append_decorations + // ); + + // costmap_2d::PluginLayerPtr plugin = creator(); + // std::cout << "Plugin created successfully" << std::endl; + + // std::string global_frame = "map"; + // bool rolling_window = true; + // bool track_unknown = true; + // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); + + // tf2::BufferCore tf_buffer; + // // tf2::Duration cache_time(10.0); + // // tf2::BufferCore tf_buffer(cache_time); + + // plugin->initialize(&layered_costmap, "inflation_layer", &tf_buffer); + // plugin->onFootprintChanged(); + + // std::cout << "Plugin initialized" << std::endl; + + + auto creator_1 = boost::dll::import_alias( + "./costmap_2d/libpreferred_layer.so", "create_plugin_static_layer", boost::dll::load_mode::append_decorations ); - costmap_2d::PluginCostmapLayerPtr plugin = creator(); + costmap_2d::PluginStaticLayerPtr plugin_1 = creator_1(); std::cout << "Plugin created successfully" << std::endl; - + std::string global_frame = "map"; bool rolling_window = true; bool track_unknown = true; LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); tf2::BufferCore tf_buffer; - // tf2::Duration cache_time(10.0); - // tf2::BufferCore tf_buffer(cache_time); - - plugin->initialize(&layered_costmap, "static_layer", &tf_buffer); - - std::cout << "Plugin initialized" << std::endl; - - // plugin->activate(); + plugin_1->initialize(&layered_costmap, "critical_layer", &tf_buffer); nav_msgs::OccupancyGrid grid; grid.info.resolution = 0.05f; @@ -43,113 +202,7 @@ int main(int argc, char* argv[]) { grid.data[1] = 100; grid.data[2] = 10; grid.data[3] = 0; - plugin->dataCallBack(grid, "map"); - - map_msgs::OccupancyGridUpdate update; - - update.x = 1; - update.y = 1; - update.width = 2; - update.height = 2; - - update.data.resize(update.width * update.height, -1); - update.data[0] = 0; - update.data[1] = 100; - update.data[2] = 10; - update.data[3] = 0; - plugin->dataCallBack(update, "map_update"); - - - creator = boost::dll::import_alias( - "./costmap_2d/libobstacle_layer.so", "create_plugin", boost::dll::load_mode::append_decorations - ); - - plugin = creator(); - std::cout << "Plugin created successfully" << std::endl; - - plugin->initialize(&layered_costmap, "obstacle_layer", &tf_buffer); - - sensor_msgs::LaserScan scan; - - // --- Header --- - scan.header.seq = 1; - scan.header.stamp = robot::Time::now(); // nếu bạn chưa có Time class, gán tạm số - scan.header.frame_id = "laser_frame"; - - // --- Cấu hình góc --- - scan.angle_min = -M_PI; // -180 độ - scan.angle_max = M_PI; // +180 độ - scan.angle_increment = M_PI / 180; // 1 độ mỗi tia - - // --- Cấu hình thời gian --- - scan.time_increment = 0.0001f; // mỗi tia cách nhau 0.1 ms - scan.scan_time = 0.04f; // tổng thời gian quét 40 ms (~25Hz) - - // --- Giới hạn đo --- - scan.range_min = 0.1f; - scan.range_max = 10.0f; - - // --- Tạo dữ liệu --- - int num_readings = static_cast((scan.angle_max - scan.angle_min) / scan.angle_increment); - scan.ranges.resize(num_readings); - scan.intensities.resize(num_readings); - - for (int i = 0; i < num_readings; ++i) - { - float angle = scan.angle_min + i * scan.angle_increment; - - // Tạo dữ liệu giả lập: vật thể cách 2m, dao động nhẹ theo góc - scan.ranges[i] = 2.0f + 0.2f * std::sin(3.0f * angle); - - // Giả lập cường độ phản xạ - scan.intensities[i] = 100.0f + 20.0f * std::cos(angle); - } - - // --- In thử 10 giá trị đầu --- - // for (int i = 0; i < 10; ++i) - // { - // std::cout << "Angle " << (scan.angle_min + i * scan.angle_increment) - // << " rad | Range " << scan.ranges[i] - // << " m | Intensity " << scan.intensities[i] - // << std::endl; - // } - plugin->deactivate(); - plugin->dataCallBack(scan, "laser_valid"); - - sensor_msgs::PointCloud cloud; - - // 2. Thiết lập header - cloud.header.stamp.sec = 1234567890; // hoặc ros::Time::now().sec - cloud.header.stamp.nsec = 0; - cloud.header.frame_id = "laser_frame"; - - // 3. Thêm một vài điểm - geometry_msgs::Point32 pt1; - pt1.x = 1.0f; pt1.y = 0.0f; pt1.z = 0.0f; - - geometry_msgs::Point32 pt2; - pt2.x = 0.0f; pt2.y = 1.0f; pt2.z = 0.0f; - - geometry_msgs::Point32 pt3; - pt3.x = 1.0f; pt3.y = 1.0f; pt3.z = 0.0f; - - cloud.points.push_back(pt1); - cloud.points.push_back(pt2); - cloud.points.push_back(pt3); - - // 4. Thêm dữ liệu channels (tùy chọn) - cloud.channels.resize(1); - cloud.channels[0].name = "intensity"; - cloud.channels[0].values.push_back(0.5f); - cloud.channels[0].values.push_back(1.0f); - cloud.channels[0].values.push_back(0.8f); - - plugin->dataCallBack(cloud, "pcl_cb"); - - - - - + plugin_1->CostmapLayer::dataCallBack(grid, "map"); - std::cout << "Plugin activated successfully" << std::endl; + std::cout << "Plugin initialized" << std::endl; }