add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,247 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_lifecycle/lifecycle_node.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/controller.hpp"
|
||||
|
||||
#include "models.hpp"
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template<typename TMessage, typename TNode>
|
||||
void setHeader(TMessage && msg, TNode node, std::string frame)
|
||||
{
|
||||
auto time = node->get_clock()->now();
|
||||
msg.header.frame_id = frame;
|
||||
msg.header.stamp = time;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
/**
|
||||
* Adds some parameters for the optimizer to a special container.
|
||||
*
|
||||
* @param params_ container for optimizer's parameters.
|
||||
*/
|
||||
void setUpOptimizerParams(
|
||||
const TestOptimizerSettings & s,
|
||||
const std::vector<std::string> & critics,
|
||||
std::vector<rclcpp::Parameter> & params_, std::string node_name = std::string("dummy"))
|
||||
{
|
||||
constexpr double dummy_freq = 50.0;
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".iteration_count", s.iteration_count));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".batch_size", s.batch_size));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".time_steps", s.time_steps));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".lookahead_dist", s.lookahead_distance));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".motion_model", s.motion_model));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".critics", critics));
|
||||
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
|
||||
}
|
||||
|
||||
void setUpControllerParams(
|
||||
bool visualize, std::vector<rclcpp::Parameter> & params_,
|
||||
std::string node_name = std::string("dummy"))
|
||||
{
|
||||
double dummy_freq = 50.0;
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".visualize", visualize));
|
||||
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
|
||||
}
|
||||
|
||||
rclcpp::NodeOptions getOptimizerOptions(
|
||||
TestOptimizerSettings s,
|
||||
const std::vector<std::string> & critics)
|
||||
{
|
||||
std::vector<rclcpp::Parameter> params;
|
||||
rclcpp::NodeOptions options;
|
||||
setUpOptimizerParams(s, critics, params);
|
||||
options.parameter_overrides(params);
|
||||
return options;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Point getDummyPoint(double x, double y)
|
||||
{
|
||||
geometry_msgs::msg::Point point;
|
||||
point.x = x;
|
||||
point.y = y;
|
||||
point.z = 0;
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos()
|
||||
{
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("cost_map_node");
|
||||
costmap_ros->on_configure(rclcpp_lifecycle::State{});
|
||||
return costmap_ros;
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2D> getDummyCostmap(TestCostmapSettings s)
|
||||
{
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2D>(
|
||||
s.cells_x, s.cells_y, s.resolution, s.origin_x, s.origin_y, s.cost_map_default_value);
|
||||
|
||||
return costmap;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> getDummySquareFootprint(double a)
|
||||
{
|
||||
return {getDummyPoint(a, a), getDummyPoint(-a, -a), getDummyPoint(a, -a), getDummyPoint(-a, a)};
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos(TestCostmapSettings s)
|
||||
{
|
||||
auto costmap_ros = getDummyCostmapRos();
|
||||
auto costmap_ptr = costmap_ros->getCostmap();
|
||||
auto costmap = getDummyCostmap(s);
|
||||
*(costmap_ptr) = *costmap;
|
||||
|
||||
costmap_ros->setRobotFootprint(getDummySquareFootprint(s.footprint_size));
|
||||
|
||||
return costmap_ros;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
|
||||
getDummyNode(
|
||||
TestOptimizerSettings s, std::vector<std::string> critics,
|
||||
std::string node_name = std::string("dummy"))
|
||||
{
|
||||
auto node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, getOptimizerOptions(s, critics));
|
||||
return node;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
|
||||
getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("dummy"))
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, options);
|
||||
return node;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TParamHandler>
|
||||
std::shared_ptr<mppi::Optimizer> getDummyOptimizer(
|
||||
TNode node, TCostMap costmap_ros,
|
||||
TParamHandler * params_handler)
|
||||
{
|
||||
std::shared_ptr<mppi::Optimizer> optimizer = std::make_shared<mppi::Optimizer>();
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
optimizer->initialize(weak_ptr_node, node->get_name(), costmap_ros, params_handler);
|
||||
|
||||
return optimizer;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TFBuffer, typename TParamHandler>
|
||||
mppi::PathHandler getDummyPathHandler(
|
||||
TNode node, TCostMap costmap_ros, TFBuffer tf_buffer,
|
||||
TParamHandler * params_handler)
|
||||
{
|
||||
mppi::PathHandler path_handler;
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
path_handler.initialize(weak_ptr_node, node->get_name(), costmap_ros, tf_buffer, params_handler);
|
||||
|
||||
return path_handler;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TFBuffer>
|
||||
std::shared_ptr<nav2_mppi_controller::MPPIController> getDummyController(
|
||||
TNode node, TFBuffer tf_buffer,
|
||||
TCostMap costmap_ros)
|
||||
{
|
||||
auto controller = std::make_shared<nav2_mppi_controller::MPPIController>();
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
controller->configure(weak_ptr_node, node->get_name(), tf_buffer, costmap_ros);
|
||||
controller->activate();
|
||||
return controller;
|
||||
}
|
||||
|
||||
auto getDummyTwist()
|
||||
{
|
||||
geometry_msgs::msg::Twist twist;
|
||||
return twist;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
geometry_msgs::msg::PoseStamped
|
||||
getDummyPointStamped(TNode & node, std::string frame = std::string("odom"))
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped point;
|
||||
detail::setHeader(point, node, frame);
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
geometry_msgs::msg::PoseStamped getDummyPointStamped(TNode & node, TestPose pose)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node);
|
||||
point.pose.position.x = pose.x;
|
||||
point.pose.position.y = pose.y;
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
nav_msgs::msg::Path getDummyPath(TNode node, std::string frame = std::string("odom"))
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
detail::setHeader(path, node, frame);
|
||||
return path;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
auto getDummyPath(size_t points_count, TNode node)
|
||||
{
|
||||
auto path = getDummyPath(node);
|
||||
|
||||
for (size_t i = 0; i < points_count; i++) {
|
||||
path.poses.push_back(getDummyPointStamped(node));
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
nav_msgs::msg::Path getIncrementalDummyPath(TNode node, TestPathSettings s)
|
||||
{
|
||||
auto path = getDummyPath(node);
|
||||
|
||||
for (size_t i = 0; i < s.poses_count; i++) {
|
||||
double x = s.start_pose.x + static_cast<double>(i) * s.step_x;
|
||||
double y = s.start_pose.y + static_cast<double>(i) * s.step_y;
|
||||
path.poses.push_back(getDummyPointStamped(node, TestPose{x, y}));
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
struct TestOptimizerSettings
|
||||
{
|
||||
int batch_size;
|
||||
int time_steps;
|
||||
int iteration_count;
|
||||
double lookahead_distance;
|
||||
std::string motion_model;
|
||||
bool consider_footprint;
|
||||
};
|
||||
|
||||
struct TestPose
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
};
|
||||
|
||||
struct TestCostmapSettings
|
||||
{
|
||||
const unsigned int cells_x = 40;
|
||||
const unsigned int cells_y = 40;
|
||||
const double origin_x = 0.0;
|
||||
const double origin_y = 0.0;
|
||||
const double resolution = 0.1;
|
||||
const unsigned char cost_map_default_value = 0;
|
||||
const double footprint_size = 0.15;
|
||||
|
||||
std::pair<unsigned int, unsigned int> getCenterIJ()
|
||||
{
|
||||
return {
|
||||
cells_x / 2,
|
||||
cells_y / 2};
|
||||
}
|
||||
|
||||
TestPose getCenterPose()
|
||||
{
|
||||
return {
|
||||
static_cast<double>(cells_x) * resolution / 2.0,
|
||||
static_cast<double>(cells_y) * resolution / 2.0};
|
||||
}
|
||||
};
|
||||
struct TestObstaclesSettings
|
||||
{
|
||||
unsigned int center_cells_x;
|
||||
unsigned int center_cells_y;
|
||||
unsigned int obstacle_size;
|
||||
unsigned char obstacle_cost;
|
||||
};
|
||||
|
||||
struct TestPathSettings
|
||||
{
|
||||
TestPose start_pose;
|
||||
unsigned int poses_count;
|
||||
double step_x;
|
||||
double step_y;
|
||||
};
|
||||
@@ -0,0 +1,248 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <string_view>
|
||||
#include <rclcpp/executors.hpp>
|
||||
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
#include "models.hpp"
|
||||
#include "factory.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
template<typename TNode>
|
||||
void waitSome(const std::chrono::nanoseconds & duration, TNode & node)
|
||||
{
|
||||
rclcpp::Time start_time = node->now();
|
||||
while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) {
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
std::this_thread::sleep_for(3ms);
|
||||
}
|
||||
}
|
||||
|
||||
void sendTf(
|
||||
std::string_view source, std::string_view dest,
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster,
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, size_t n)
|
||||
{
|
||||
while (--n != 0u) {
|
||||
auto t = geometry_msgs::msg::TransformStamped();
|
||||
t.header.frame_id = source;
|
||||
t.child_frame_id = dest;
|
||||
|
||||
t.header.stamp = node->now() + rclcpp::Duration(3ms);
|
||||
t.transform.translation.x = 0.0;
|
||||
t.transform.translation.y = 0.0;
|
||||
t.transform.translation.z = 0.0;
|
||||
t.transform.rotation.x = 0.0;
|
||||
t.transform.rotation.y = 0.0;
|
||||
t.transform.rotation.z = 0.0;
|
||||
t.transform.rotation.w = 1.0;
|
||||
|
||||
tf_broadcaster->sendTransform(t);
|
||||
|
||||
// Allow tf_buffer_ to be filled by listener
|
||||
waitSome(10ms, node);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Print costmap to stdout.
|
||||
* @param costmap map to be printed.
|
||||
*/
|
||||
void printMap(const nav2_costmap_2d::Costmap2D & costmap)
|
||||
{
|
||||
for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) {
|
||||
for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) {
|
||||
printf("%4d", static_cast<int>(costmap.getCost(j, i)));
|
||||
}
|
||||
printf("\n\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Print costmap with trajectory and goal point to stdout.
|
||||
* @param costmap map to be printed.
|
||||
* @param trajectory trajectory container (xt::tensor) to be printed.
|
||||
* @param goal_point goal point to be printed.
|
||||
*/
|
||||
template<typename TTrajectory>
|
||||
void printMapWithTrajectoryAndGoal(
|
||||
nav2_costmap_2d::Costmap2D & costmap, const TTrajectory & trajectory,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
const unsigned int trajectory_cost = 1;
|
||||
const unsigned int goal_cost = 2;
|
||||
|
||||
std::cout << "Costmap: \n trajectory = " << trajectory_cost << "\n goal = " << goal_cost
|
||||
<< "\n obsctacle = 255 \n";
|
||||
|
||||
// create new costmap
|
||||
nav2_costmap_2d::Costmap2D costmap2d(
|
||||
costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getResolution(),
|
||||
costmap.getOriginX(), costmap.getOriginY(), costmap.getDefaultValue());
|
||||
|
||||
// copy obstacles from original costmap
|
||||
costmap2d = costmap;
|
||||
|
||||
// add trajectory on map
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
for (size_t i = 0; i < trajectory.shape()[0]; ++i) {
|
||||
costmap2d.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
|
||||
costmap2d.setCost(point_mx, point_my, trajectory_cost);
|
||||
}
|
||||
|
||||
unsigned int goal_j{0};
|
||||
unsigned int goal_i{0};
|
||||
costmap2d.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
|
||||
std::cout << "Goal Position: " << goal_j << " " << goal_i << "\n";
|
||||
costmap2d.setCost(goal_j, goal_i, goal_cost);
|
||||
printMap(costmap2d);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a square obstacle to the costmap.
|
||||
* @param costmap map to be modified.
|
||||
* @param upper_left_corner_x obstacle upper left corner X coord (on the
|
||||
* costmap).
|
||||
* @param upper_left_corner_y obstacle upper left corner Y coord (on the
|
||||
* costmap).
|
||||
* @param size obstacle side size.
|
||||
* @param cost obstacle value on costmap.
|
||||
*/
|
||||
void addObstacle(
|
||||
nav2_costmap_2d::Costmap2D * costmap, unsigned int upper_left_corner_x,
|
||||
unsigned int upper_left_corner_y, unsigned int size, unsigned char cost)
|
||||
{
|
||||
for (unsigned int i = upper_left_corner_x; i < upper_left_corner_x + size; i++) {
|
||||
for (unsigned int j = upper_left_corner_y; j < upper_left_corner_y + size; j++) {
|
||||
costmap->setCost(i, j, cost);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void printInfo(
|
||||
TestOptimizerSettings os, TestPathSettings ps,
|
||||
const std::vector<std::string> & critics)
|
||||
{
|
||||
std::stringstream ss;
|
||||
for (auto str : critics) {
|
||||
ss << str << " ";
|
||||
}
|
||||
|
||||
std::cout << //
|
||||
"\n\n--------------------OPTIMIZER OPTIONS-----------------------------\n" <<
|
||||
"Critics: " << ss.str() << "\n" \
|
||||
"Motion model: " << os.motion_model << "\n"
|
||||
"Consider footprint: " << os.consider_footprint << "\n" <<
|
||||
"Iterations: " << os.iteration_count << "\n" <<
|
||||
"Batch size: " << os.batch_size << "\n" <<
|
||||
"Time steps: " << os.time_steps << "\n" <<
|
||||
"Path points: " << ps.poses_count << "\n" <<
|
||||
"\n-------------------------------------------------------------------\n\n";
|
||||
}
|
||||
|
||||
void addObstacle(nav2_costmap_2d::Costmap2D * costmap, TestObstaclesSettings s)
|
||||
{
|
||||
addObstacle(costmap, s.center_cells_x, s.center_cells_y, s.obstacle_size, s.obstacle_cost);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check the trajectory for collisions with obstacles on the map.
|
||||
* @param trajectory trajectory container (xt::tensor) to be checked.
|
||||
* @param costmap costmap with obstacles
|
||||
* @return true - if the trajectory crosses an obstacle on the map, false - if
|
||||
* not
|
||||
*/
|
||||
template<typename TTrajectory>
|
||||
bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap)
|
||||
{
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
|
||||
for (size_t i = 0; i < trajectory.shape(0); ++i) {
|
||||
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
|
||||
auto cost_ = costmap.getCost(point_mx, point_my);
|
||||
if (cost_ > nav2_costmap_2d::FREE_SPACE || cost_ == nav2_costmap_2d::NO_INFORMATION) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned char getCost(const nav2_costmap_2d::Costmap2D & costmap, double x, double y)
|
||||
{
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
|
||||
costmap.worldToMap(x, y, point_mx, point_my);
|
||||
return costmap.getCost(point_mx, point_my);
|
||||
}
|
||||
|
||||
template<typename TTrajectory>
|
||||
bool isGoalReached(
|
||||
const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
unsigned int trajectory_j = 0;
|
||||
unsigned int trajectory_i = 0;
|
||||
|
||||
unsigned int goal_j = 0;
|
||||
unsigned int goal_i = 0;
|
||||
costmap.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
|
||||
|
||||
auto match = [](unsigned int i, unsigned int j, unsigned int i_dst, unsigned int j_dst) {
|
||||
if (i == i_dst && j == j_dst) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
|
||||
auto match_near = [&](unsigned int i, unsigned int j) {
|
||||
if (match(i, j, goal_i, goal_j) ||
|
||||
match(i, j, goal_i + 1, goal_j) ||
|
||||
match(i, j, goal_i - 1, goal_j) ||
|
||||
match(i, j, goal_i, goal_j + 1) ||
|
||||
match(i, j, goal_i, goal_j - 1) ||
|
||||
match(i, j, goal_i + 1, goal_j + 1) ||
|
||||
match(i, j, goal_i + 1, goal_j - 1) ||
|
||||
match(i, j, goal_i - 1, goal_j + 1) ||
|
||||
match(i, j, goal_i - 1, goal_j - 1))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
// clang-format on
|
||||
|
||||
for (size_t i = 0; i < trajectory.shape(0); ++i) {
|
||||
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), trajectory_j, trajectory_i);
|
||||
if (match_near(trajectory_i, trajectory_j)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
Reference in New Issue
Block a user