DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update folder plugins

Browse Source
This commit is contained in:
duongtd 2025-11-13 17:39:09 +07:00
parent c94de60a7b
commit bd98bf4e08
18 changed files with 1008 additions and 610 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
CMakeLists.txt
View File

@ -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

282
include/costmap_2d/costmap_2d_robot.h Normal file
View File

@ -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 <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/Costmap2DConfig.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <dynamic_reconfigure/server.h>
#include <pluginlib/class_loader.hpp>
#include <tf2/LinearMath/Transform.h>
// 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<XmlRpc::XmlRpcValue>(*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<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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<geometry_msgs::Point>& 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 &param_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<costmap_2d::Layer> plugin_loader_;
// Costmap2DPublisher* publisher_;
// dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_;
// ros::Subscriber footprint_sub_;
// ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
// costmap_2d::Costmap2DConfig old_config_;
};
// class Costmap2DRobot
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_ROS_H

4
include/costmap_2d/critical_layer.h
View File

@ -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;
};
}

58
include/costmap_2d/data_convert.h
View File

@ -2,12 +2,60 @@
#define DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/utils.h>
#include <tf2/compat.h>
#include <robot/time.h>
#include <cmath>
namespace costmap_2d
{
template <typename T>
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)
{

12
include/costmap_2d/directional_layer.h
View File

@ -2,8 +2,7 @@
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
#include <costmap_2d/static_layer.h>
// #include <tf/transform_listener.h>
// #include <nav_msgs/Path.h>
#include <nav_msgs/Path.h>
namespace costmap_2d
{
@ -12,14 +11,14 @@ namespace costmap_2d
public:
DirectionalLayer();
virtual ~DirectionalLayer();
// bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
bool laneFilter(const std::vector<geometry_msgs::PoseStamped> plan);
void resetMap();
private:
// void incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map);
// bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
void incomingMap(const nav_msgs::OccupancyGrid &new_map);
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
unsigned char laneFilter(unsigned int x, unsigned int y, double yaw_robot);
// void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
void laneFilter(std::vector<unsigned int> x, std::vector<unsigned int> y, std::vector<double> yaw_robot);
void laneFilter(std::vector<std::pair<unsigned int, double>> x, std::vector<std::pair<unsigned int, double>> y, std::vector<double> 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_;
};
}

36
include/costmap_2d/obstacle_layer.h
View File

@ -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<costmap_2d::ObservationBuffer>& 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<ObservationBuffer>& 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<costmap_2d::ObservationBuffer>& 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<costmap_2d::ObservationBuffer>& 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<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_; ///< @brief Used for the observation message filters
// std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensors
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles
std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles
@ -183,6 +149,8 @@ private:
bool getParams();
};
using PluginObstacleLayerPtr = std::shared_ptr<ObstacleLayer>;
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSTACLE_LAYER_H_

30
include/costmap_2d/voxel_grid.h
View File

@ -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 <std_msgs/Header.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/Vector3.h>
namespace costmap_2d
{
struct VoxelGrid
{
std_msgs::Header header;
std::vector<uint32_t> 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_

2
include/costmap_2d/voxel_layer.h
View File

@ -42,7 +42,7 @@
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/voxel_grid.h>
// #include <costmap_2d/voxel_grid.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
// #include <laser_geometry/laser_geometry.h>

6
plugins/critical_layer.cpp
View File

@ -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)
}

822
plugins/directional_layer.cpp
View File

@ -1,11 +1,9 @@
#include <costmap_2d/directional_layer.h>
// #include <pluginlib/class_list_macros.hpp>
// #include <tf/transform_listener.h>
// #include <tf2/convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf/transform_datatypes.h>
#include <costmap_2d/data_convert.h>
#include <tf2/convert.h>
#include <tf2/utils.h>
// PLUGINLIB_EXPORT_CLASS(costmap_2d::DirectionalLayer, costmap_2d::Layer)
#include <boost/dll/alias.hpp>
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
@ -15,398 +13,426 @@ using costmap_2d::NO_INFORMATION;
namespace costmap_2d
{
// DirectionalLayer::DirectionalLayer()
// {
DirectionalLayer::DirectionalLayer()
{
// ros::NodeHandle nh("~/" + name_);
// lane_mask_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/direction_zones/lanes", 1);
// }
// DirectionalLayer::~DirectionalLayer() {}
// bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> 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);
// }
// void DirectionalLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr &new_map)
// {
// 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);
// // 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;
// // 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_;
// // 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<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
// {
// boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
// if (new_map.empty())
// return false;
// std::vector<std::pair<unsigned int, double>> X_List, Y_List;
// std::vector<double> Yaw_list;
// 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<unsigned int, double> x_val(mx, p.pose.position.x);
// std::pair<unsigned int, double> 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;
// }
// 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;
// }
// }
// 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;
// }
// void DirectionalLayer::laneFilter(std::vector<std::pair<unsigned int, double>> x,
// std::vector<std::pair<unsigned int, double>> y,
// std::vector<double> 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;
// 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);
// }
// // 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;
// 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 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;
// }
// }
// }
// 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::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;
// }
}
DirectionalLayer::~DirectionalLayer() {}
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> 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::OccupancyGrid& new_map)
{
if(!map_shutdown_)
{
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
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)
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;
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();
}
}
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
{
boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
if (new_map.empty())
return false;
std::vector<std::pair<unsigned int, double>> X_List, Y_List;
std::vector<double> Yaw_list;
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))
{
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<unsigned int, double> x_val(mx, p.pose.position.x);
std::pair<unsigned int, double> 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 = 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;
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<std::pair<unsigned int, double>> x,
std::vector<std::pair<unsigned int, double>> y,
std::vector<double> 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;
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);
}
// 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;
}
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;
}
}
}
}
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)
{
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;
}
// Export factory function
static PluginStaticLayerPtr create_directional_plugin() {
return std::make_shared<DirectionalLayer>();
}
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_directional_plugin, create_plugin_static_layer)
}

15
plugins/inflation_layer.cpp
View File

@ -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_);
}

8
plugins/obstacle_layer.cpp
View File

@ -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

5
plugins/preferred_layer.cpp
View File

@ -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<PreferredLayer>();
}
// 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)
}

4
plugins/unpreferred_layer.cpp
View File

@ -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<UnPreferredLayer>();
}
// 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)
}

20
plugins/voxel_layer.cpp
View File

@ -53,9 +53,9 @@ using costmap_2d::Observation;
namespace costmap_2d
{
// void VoxelLayer::onInitialize()
// {
// ObstacleLayer::onInitialize();
void VoxelLayer::onInitialize()
{
ObstacleLayer::onInitialize();
// ros::NodeHandle private_nh("~/" + name_);
// private_nh.param("publish_voxel_map", publish_voxel_, false);
@ -63,7 +63,7 @@ namespace costmap_2d
// voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);
// clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);
// }
}
// void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
// {
@ -90,12 +90,12 @@ VoxelLayer::~VoxelLayer()
// matchSize();
// }
// void VoxelLayer::matchSize()
// {
// ObstacleLayer::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::reset()
// {
@ -445,11 +445,11 @@ VoxelLayer::~VoxelLayer()
// }
// Export factory function
static PluginPtr create_voxel_plugin() {
static PluginObstacleLayerPtr create_voxel_plugin() {
return std::make_shared<VoxelLayer>();
}
// 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

291
test/static_layer_test.cpp
View File

@ -7,15 +7,181 @@
#include "nav_msgs/OccupancyGrid.h"
#include <map_msgs/OccupancyGridUpdate.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/critical_layer.h>
#include <costmap_2d/preferred_layer.h>
using namespace costmap_2d;
int main(int argc, char* argv[]) {
auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
"./costmap_2d/libstatic_layer.so", "create_plugin", boost::dll::load_mode::append_decorations
// auto creator = boost::dll::import_alias<costmap_2d::PluginCostmapLayerPtr()>(
// "./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::PluginCostmapLayerPtr()>(
// "./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<int>((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::PluginLayerPtr()>(
// "./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::PluginStaticLayerPtr()>(
"./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";
@ -24,14 +190,7 @@ int main(int argc, char* argv[]) {
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");
plugin_1->CostmapLayer::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::PluginCostmapLayerPtr()>(
"./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<int>((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;
std::cout << "Plugin initialized" << std::endl;
}