41 #include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
45 #include "nav2_util/geometry_utils.hpp"
46 #include "nav2_util/robot_utils.hpp"
47 #include "geometry_msgs/msg/point.hpp"
49 #include "nav2_costmap_2d/cost_values.hpp"
50 #include "nav2_util/occ_grid_values.hpp"
56 : filter_info_topic_(
""), mask_topic_(
"")
68 nav2::LifecycleNode::SharedPtr node = node_.lock();
70 throw std::runtime_error{
"Failed to lock node"};
75 enabled_ = node->declare_or_get_parameter(name_ +
"." +
"enabled",
true);
78 double transform_tolerance = node->declare_or_get_parameter(name_ +
"." +
"transform_tolerance",
82 name_ +
".transform_staleness_threshold", 0.0);
86 name_ +
"/toggle_filter",
89 std::placeholders::_2, std::placeholders::_3));
90 }
catch (
const std::exception & ex) {
91 RCLCPP_ERROR(logger_,
"Parameter problem: %s", ex.what());
114 double robot_x,
double robot_y,
double robot_yaw,
115 double * ,
double * ,
double * ,
double * )
121 latest_pose_.position.x = robot_x;
122 latest_pose_.position.y = robot_y;
123 latest_pose_.position.z = 0.0;
124 latest_pose_.orientation = nav2_util::geometry_utils::orientationAroundZAxis(robot_yaw);
129 int min_i,
int min_j,
int max_i,
int max_j)
135 process(master_grid, min_i, min_j, max_i, max_j, latest_pose_);
140 const std::shared_ptr<rmw_request_id_t>,
141 const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
142 std::shared_ptr<std_srvs::srv::SetBool::Response> response)
144 enabled_ = request->data;
145 response->success =
true;
147 response->message =
"Enabled";
149 response->message =
"Disabled";
154 const std::string global_frame,
155 const geometry_msgs::msg::Pose & global_pose,
156 const std::string mask_frame,
157 geometry_msgs::msg::Pose & mask_pose)
const
159 if (mask_frame != global_frame) {
163 geometry_msgs::msg::TransformStamped global_to_mask_transform;
164 geometry_msgs::msg::Point in, out;
165 in.x = global_pose.position.x;
166 in.y = global_pose.position.y;
169 if (!nav2_util::lookupTransformWithStalenessCheck(
171 global_to_mask_transform))
175 tf2::doTransform(in, out, global_to_mask_transform);
177 mask_pose = global_pose;
178 mask_pose.position.x = out.x;
179 mask_pose.position.y = out.y;
185 mask_pose = global_pose;
192 nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask,
193 const unsigned int mx,
const unsigned int & my)
const
195 const unsigned int index = my * filter_mask->info.width + mx;
197 const signed char data = filter_mask->data[index];
198 if (data == nav2_util::OCC_GRID_UNKNOWN) {
199 return NO_INFORMATION;
204 static_cast<double>(data) * (LETHAL_OBSTACLE - FREE_SPACE) /
205 (nav2_util::OCC_GRID_OCCUPIED - nav2_util::OCC_GRID_FREE));
A 2D costmap provides a mapping between points in the world and their associated "costs".
virtual void process(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j, const geometry_msgs::msg::Pose &pose)=0
: An algorithm for how to use that map's information. Fills the Costmap2D with calculated data and ma...
void activate() final
Activate the layer.
std::recursive_mutex mutex_t
: Provide a typedef to ease future code maintenance
nav2::ServiceServer< std_srvs::srv::SetBool >::SharedPtr enable_service_
: A service to enable/disable costmap filter
void deactivate() final
Deactivate the layer.
bool transformPose(const std::string global_frame, const geometry_msgs::msg::Pose &global_pose, const std::string mask_frame, geometry_msgs::msg::Pose &mask_pose) const
: Transforms robot position using latest TF, rejecting stale dynamic TF when transform_staleness_thre...
void reset() final
Reset the layer.
virtual void initializeFilter(const std::string &filter_info_topic)=0
: Initializes costmap filter. Creates subscriptions to filter-related topics
double transform_staleness_threshold_
: Max age for transforms. Check disabled if <= 0.
CostmapFilter()
A constructor.
void updateCosts(nav2_costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) final
Update the costs in the master costmap in the window.
std::string filter_info_topic_
: Name of costmap filter info topic
virtual void resetFilter()=0
: Resets costmap filter. Stops all subscriptions
tf2::Duration transform_tolerance_
: mask_frame->global_frame_ transform tolerance
unsigned char getMaskCost(nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, const unsigned int mx, const unsigned int &my) const
Get the cost of a cell in the filter mask.
void onInitialize() final
Initialization process of layer on startup.
void enableCallback(const std::shared_ptr< rmw_request_id_t > request_header, const std::shared_ptr< std_srvs::srv::SetBool::Request > request, std::shared_ptr< std_srvs::srv::SetBool::Response > response)
Costmap filter enabling/disabling callback.
~CostmapFilter()
A destructor.
void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y) override
Update the bounds of the master costmap by this layer's update dimensions.
void setCurrent(bool current)
Set whether the data in the layer is up to date.