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

first commit

Browse Source
This commit is contained in:
duongtd
2025-10-28 13:43:40 +07:00
parent 63dc18f3f0
commit 5d4d1943fe
56 changed files with 1660 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/costmap_2d/costmap_layer.h
View File

@@ -37,7 +37,6 @@
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_LAYER_H_
#define COSTMAP_2D_COSTMAP_LAYER_H_
#include <ros/ros.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>

19
include/costmap_2d/msg.h
View File

@@ -2,8 +2,18 @@
#define COSTMAP_2D_COST_VALUES_H_MSG
/** Provides a mapping for often used cost values */
#include<vector>
#include<string>
#include<chrono>
namespace costmap_2d
{
struct Header
{
uint32_t seq;
std::chrono::system_clock::time_point stamp;
std::string frame_id;
};
struct Point
{
double x;
@@ -18,6 +28,12 @@ namespace costmap_2d
float z;
};
struct PointStamped
{
Header header;
Point point;
};
struct Polygon
{
std::vector<Point32> points;
@@ -52,6 +68,7 @@ namespace costmap_2d
struct PointCloud2
{
Header header;
uint32_t height;
uint32_t width;
std::vector<PointField> fields;
@@ -62,5 +79,7 @@ namespace costmap_2d
bool is_dense;
};
}
#endif // COSTMAP_2D_COST_VALUES_H_MSG

102
include/costmap_2d/observation.h Normal file
View File

@@ -0,0 +1,102 @@
/*
* 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 the 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.
*
* Authors: Conor McGann
*/
#ifndef COSTMAP_2D_OBSERVATION_H_
#define COSTMAP_2D_OBSERVATION_H_
#include <msg/PointCloud2.h>
#include <geometry_msgs/Point.h>
namespace costmap_2d
{
/**
* @brief Stores an observation in terms of a point cloud and the origin of the source
* @note Tried to make members and constructor arguments const but the compiler would not accept the default
* assignment operator for vector insertion!
*/
class Observation
{
public:
/**
* @brief Creates an empty observation
*/
Observation() :
cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
{
}
virtual ~Observation()
{
delete cloud_;
}
/**
* @brief Creates an observation from an origin point and a point cloud
* @param origin The origin point of the observation
* @param cloud The point cloud of the observation
* @param obstacle_range The range out to which an observation should be able to insert obstacles
* @param raytrace_range The range out to which an observation should be able to clear via raytracing
*/
Observation(geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
double obstacle_range, double raytrace_range) :
origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
{
}
/**
* @brief Copy constructor
* @param obs The observation to copy
*/
Observation(const Observation& obs) :
origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
{
}
/**
* @brief Creates an observation from a point cloud
* @param cloud The point cloud of the observation
* @param obstacle_range The range out to which an observation should be able to insert obstacles
*/
Observation(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
{
}
geometry_msgs::Point origin_;
sensor_msgs::PointCloud2* cloud_;
double obstacle_range_, raytrace_range_;
};
} // namespace costmap_2d
#endif // COSTMAP_2D_OBSERVATION_H_

64
include/costmap_2d/observation_buffer.h
View File

@@ -1,50 +1,20 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_
#define COSTMAP_2D_OBSERVATION_BUFFER_H_
#include <vector>
#include <list>
#include <string>
#include <ros/time.h>
#include <costmap_2d/observation.h>
#include <tf2_ros/buffer.h>
#include <chrono>
// #include <ros/time.h>
// #include <costmap_2d/observation.h>
// #include <tf2/buffer_core.h>
#include <costmap_2d/observation.h>
#include <tf2/buffer_core.h>
#include <msg/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
// Thread support
#include <boost/thread.hpp>
@@ -58,6 +28,7 @@ namespace costmap_2d
class ObservationBuffer
{
public:
/**
* @brief Constructs an observation buffer
* @param topic_name The topic of the observations, used as an identifier for error and warning messages
@@ -74,7 +45,7 @@ public:
*/
ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2_ros::Buffer& tf2_buffer, std::string global_frame,
double raytrace_range, tf2::BufferCore& tf2_buffer, std::string global_frame,
std::string sensor_frame, double tf_tolerance);
/**
@@ -137,10 +108,13 @@ private:
*/
void purgeStaleObservations();
tf2_ros::Buffer& tf2_buffer_;
const ros::Duration observation_keep_time_;
const ros::Duration expected_update_rate_;
ros::Time last_updated_;
tf2::BufferCore& tf2_buffer_;
// const ros::Duration observation_keep_time_;
// const ros::Duration expected_update_rate_;
// ros::Time last_updated_;
const std::chrono::duration<double> observation_keep_time_;
const std::chrono::duration<double> expected_update_rate_;
std::chrono::time_point<std::chrono::system_clock> last_updated_;
std::string global_frame_;
std::string sensor_frame_;
std::list<Observation> observation_list_;