hiep sua ten file
This commit is contained in:
parent
2c3d7d586d
commit
4246453ae6
|
|
@ -56,13 +56,13 @@ add_library(costmap_2d
|
|||
# --- Link các thư viện phụ thuộc ---
|
||||
target_link_libraries(costmap_2d
|
||||
${Boost_LIBRARIES} # Boost
|
||||
std_msgs
|
||||
sensor_msgs
|
||||
robot_std_msgs
|
||||
robot_sensor_msgs
|
||||
geometry_msgs
|
||||
nav_msgs
|
||||
map_msgs
|
||||
robot_nav_msgs
|
||||
robot_map_msgs
|
||||
laser_geometry
|
||||
visualization_msgs
|
||||
robot_visualization_msgs
|
||||
voxel_grid
|
||||
tf3
|
||||
tf3_geometry_msgs
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
#define COSTMAP_2D_DIRECTIONAL_LAYER_V1_H_
|
||||
|
||||
#include <costmap_2d/static_layer.h>
|
||||
#include <nav_msgs/Path.h>
|
||||
#include <robot_nav_msgs/Path.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -15,20 +15,20 @@ namespace costmap_2d
|
|||
void resetMap();
|
||||
|
||||
private:
|
||||
void incomingMap(const nav_msgs::OccupancyGrid &new_map);
|
||||
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path);
|
||||
void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
|
||||
bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_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<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);
|
||||
void convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th);
|
||||
bool getRobotPose(double &x, double &y, double &yaw);
|
||||
|
||||
std::vector<std::array<uint16_t, 2>> directional_areas_;
|
||||
double pose_x_, pose_y_, pose_yaw_;
|
||||
double distance_skip_ = 1.0;
|
||||
// ros::Publisher lane_mask_pub_;
|
||||
nav_msgs::OccupancyGrid new_map_;
|
||||
robot_nav_msgs::OccupancyGrid new_map_;
|
||||
};
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
#define COSTMAP_2D_OBSERVATION_H_
|
||||
|
||||
#include <robot_geometry_msgs/Point.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -50,7 +50,7 @@ public:
|
|||
* @brief Creates an empty observation
|
||||
*/
|
||||
Observation() :
|
||||
cloud_(new sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
|
||||
cloud_(new robot_sensor_msgs::PointCloud2()), obstacle_range_(0.0), raytrace_range_(0.0)
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -66,9 +66,9 @@ public:
|
|||
* @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(robot_geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
|
||||
Observation(robot_geometry_msgs::Point& origin, const robot_sensor_msgs::PointCloud2 &cloud,
|
||||
double obstacle_range, double raytrace_range) :
|
||||
origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
|
||||
origin_(origin), cloud_(new robot_sensor_msgs::PointCloud2(cloud)),
|
||||
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range)
|
||||
{
|
||||
}
|
||||
|
|
@ -78,7 +78,7 @@ public:
|
|||
* @param obs The observation to copy
|
||||
*/
|
||||
Observation(const Observation& obs) :
|
||||
origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
|
||||
origin_(obs.origin_), cloud_(new robot_sensor_msgs::PointCloud2(*(obs.cloud_))),
|
||||
obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_)
|
||||
{
|
||||
}
|
||||
|
|
@ -88,13 +88,13 @@ public:
|
|||
* @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)
|
||||
Observation(const robot_sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
|
||||
cloud_(new robot_sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0)
|
||||
{
|
||||
}
|
||||
|
||||
robot_geometry_msgs::Point origin_;
|
||||
sensor_msgs::PointCloud2* cloud_;
|
||||
robot_sensor_msgs::PointCloud2* cloud_;
|
||||
double obstacle_range_, raytrace_range_;
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -44,7 +44,7 @@
|
|||
#include <costmap_2d/observation.h>
|
||||
#include <tf3/buffer_core.h>
|
||||
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
|
||||
// Thread support
|
||||
#include <boost/thread.hpp>
|
||||
|
|
@ -96,7 +96,7 @@ public:
|
|||
* <b>Note: The burden is on the user to make sure the transform is available... ie they should use a MessageNotifier</b>
|
||||
* @param cloud The cloud to be buffered
|
||||
*/
|
||||
void bufferCloud(const sensor_msgs::PointCloud2& cloud);
|
||||
void bufferCloud(const robot_sensor_msgs::PointCloud2& cloud);
|
||||
|
||||
/**
|
||||
* @brief Pushes copies of all current observations onto the end of the vector passed in
|
||||
|
|
|
|||
|
|
@ -43,13 +43,13 @@
|
|||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/footprint.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <robot_sensor_msgs/LaserScan.h>
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
#include <robot_sensor_msgs/PointCloud.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
||||
|
||||
namespace costmap_2d
|
||||
{
|
||||
|
|
@ -86,7 +86,7 @@ protected:
|
|||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void laserScanCallback(const sensor_msgs::LaserScan& message,
|
||||
void laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
|
|
@ -94,7 +94,7 @@ protected:
|
|||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void laserScanValidInfCallback(const sensor_msgs::LaserScan& message,
|
||||
void laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
|
|
@ -102,7 +102,7 @@ protected:
|
|||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
void pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
|
|
@ -110,7 +110,7 @@ protected:
|
|||
* @param message The message returned from a message notifier
|
||||
* @param buffer A pointer to the observation buffer to update
|
||||
*/
|
||||
void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
|
||||
void pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
||||
const boost::shared_ptr<costmap_2d::ObservationBuffer>& buffer);
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -41,8 +41,8 @@
|
|||
#include <costmap_2d/costmap_layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_map_msgs/OccupancyGridUpdate.h>
|
||||
#include <string>
|
||||
|
||||
namespace costmap_2d
|
||||
|
|
@ -68,8 +68,8 @@ protected:
|
|||
const std::type_info& type,
|
||||
const std::string& topic) override;
|
||||
virtual unsigned char interpretValue(unsigned char value);
|
||||
virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
|
||||
virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
|
||||
virtual void incomingMap(const robot_nav_msgs::OccupancyGrid& new_map);
|
||||
virtual void incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update);
|
||||
unsigned char* threshold_;
|
||||
std::string base_frame_id_;
|
||||
std::string global_frame_; ///< @brief The global frame for the costmap
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@
|
|||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/inflation_layer.h>
|
||||
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
const double MAX_Z(1.0);
|
||||
|
||||
|
|
@ -66,13 +66,13 @@ costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers,
|
|||
|
||||
void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, double z = 0.0,
|
||||
double ox = 0.0, double oy = 0.0, double oz = MAX_Z){
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
sensor_msgs::PointCloud2Modifier modifier(cloud);
|
||||
robot_sensor_msgs::PointCloud2 cloud;
|
||||
robot_sensor_msgs::PointCloud2Modifier modifier(cloud);
|
||||
modifier.setPointCloud2FieldsByString(1, "xyz");
|
||||
modifier.resize(1);
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
|
||||
robot_sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x");
|
||||
robot_sensor_msgs::PointCloud2Iterator<float> iter_y(cloud, "y");
|
||||
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(cloud, "z");
|
||||
*iter_x = x;
|
||||
*iter_y = y;
|
||||
*iter_z = z;
|
||||
|
|
|
|||
|
|
@ -10,7 +10,7 @@
|
|||
#define VOXEL_GRID_COSTMAP_2D_H
|
||||
|
||||
#include <vector>
|
||||
#include <std_msgs/Header.h>
|
||||
#include <robot_std_msgs/Header.h>
|
||||
#include <robot_geometry_msgs/Point32.h>
|
||||
#include <robot_geometry_msgs/Vector3.h>
|
||||
|
||||
|
|
@ -18,7 +18,7 @@ namespace costmap_2d
|
|||
{
|
||||
struct VoxelGrid
|
||||
{
|
||||
std_msgs::Header header;
|
||||
robot_std_msgs::Header header;
|
||||
std::vector<uint32_t> data;
|
||||
robot_geometry_msgs::Point32 origin;
|
||||
robot_geometry_msgs::Vector3 resolutions;
|
||||
|
|
|
|||
|
|
@ -42,12 +42,12 @@
|
|||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/observation_buffer.h>
|
||||
#include <costmap_2d/voxel_grid.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include <robot_nav_msgs/OccupancyGrid.h>
|
||||
#include <robot_sensor_msgs/LaserScan.h>
|
||||
#include <laser_geometry/laser_geometry.hpp>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
#include <robot_sensor_msgs/PointCloud.h>
|
||||
#include <robot_sensor_msgs/PointCloud2.h>
|
||||
#include <robot_sensor_msgs/point_cloud_conversion.h>
|
||||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <voxel_grid/voxel_grid.h>
|
||||
|
||||
|
|
@ -93,7 +93,7 @@ private:
|
|||
voxel_grid::VoxelGrid voxel_grid_;
|
||||
double z_resolution_, origin_z_;
|
||||
unsigned int unknown_threshold_, mark_threshold_, size_z_;
|
||||
sensor_msgs::PointCloud clearing_endpoints_;
|
||||
robot_sensor_msgs::PointCloud clearing_endpoints_;
|
||||
|
||||
inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -23,14 +23,14 @@ namespace costmap_2d
|
|||
{
|
||||
if (directional_areas_.empty())
|
||||
throw "Has no map data";
|
||||
nav_msgs::Path path;
|
||||
robot_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)
|
||||
void DirectionalLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
|
||||
{
|
||||
if(!map_shutdown_)
|
||||
{
|
||||
|
|
@ -110,7 +110,7 @@ namespace costmap_2d
|
|||
}
|
||||
}
|
||||
|
||||
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const nav_msgs::Path path)
|
||||
bool DirectionalLayer::laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path)
|
||||
{
|
||||
boost::unique_lock<mutex_t> lock(*layered_costmap_->getCostmap()->getMutex());
|
||||
if (new_map.empty())
|
||||
|
|
@ -159,7 +159,7 @@ namespace costmap_2d
|
|||
if (!Yaw_list.empty())
|
||||
{
|
||||
laneFilter(X_List, Y_List, Yaw_list);
|
||||
nav_msgs::OccupancyGrid lanes;
|
||||
robot_nav_msgs::OccupancyGrid lanes;
|
||||
convertToMap(costmap_, lanes, 0.65, 0.196);
|
||||
|
||||
//////////////////////////////////
|
||||
|
|
@ -177,7 +177,7 @@ namespace costmap_2d
|
|||
|
||||
}
|
||||
|
||||
void DirectionalLayer::convertToMap(unsigned char *costmap, nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
|
||||
void DirectionalLayer::convertToMap(unsigned char *costmap, robot_nav_msgs::OccupancyGrid &dir_map, double occ_th, double free_th)
|
||||
{
|
||||
dir_map.header = new_map_.header;
|
||||
dir_map.header.stamp = robot::Time::now();
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
#include <costmap_2d/obstacle_layer.h>
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
#include <tf3/exceptions.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
|
@ -199,14 +199,14 @@ void ObstacleLayer::handleImpl(const void* data,
|
|||
{
|
||||
if(observation_buffers_.empty()) return;
|
||||
boost::shared_ptr<ObservationBuffer> buffer = observation_buffers_.back();
|
||||
if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
|
||||
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
|
||||
laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
|
||||
pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
|
||||
} else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
|
||||
pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(data), buffer);
|
||||
if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser") {
|
||||
laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(robot_sensor_msgs::LaserScan) && topic == "laser_valid") {
|
||||
laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
|
||||
} else if (type == typeid(robot_sensor_msgs::PointCloud) && topic == "pcl_cb") {
|
||||
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
|
||||
} else if (type == typeid(robot_sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
|
||||
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
|
||||
} else {
|
||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
|
|
@ -218,11 +218,11 @@ void ObstacleLayer::handleImpl(const void* data,
|
|||
}
|
||||
}
|
||||
|
||||
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
|
||||
void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// project the laser into a point cloud
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
robot_sensor_msgs::PointCloud2 cloud;
|
||||
cloud.header = message.header;
|
||||
|
||||
// project the scan into a point cloud
|
||||
|
|
@ -248,12 +248,12 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
|
|||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_message,
|
||||
void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan& raw_message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// Filter positive infinities ("Inf"s) to max_range.
|
||||
float epsilon = 0.0001; // a tenth of a millimeter
|
||||
sensor_msgs::LaserScan message = raw_message;
|
||||
robot_sensor_msgs::LaserScan message = raw_message;
|
||||
for (size_t i = 0; i < message.ranges.size(); i++)
|
||||
{
|
||||
float range = message.ranges[ i ];
|
||||
|
|
@ -264,7 +264,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
|
|||
}
|
||||
|
||||
// project the laser into a point cloud
|
||||
sensor_msgs::PointCloud2 cloud;
|
||||
robot_sensor_msgs::PointCloud2 cloud;
|
||||
cloud.header = message.header;
|
||||
|
||||
// project the scan into a point cloud
|
||||
|
|
@ -290,12 +290,12 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
|
|||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
|
||||
void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
sensor_msgs::PointCloud2 cloud2;
|
||||
robot_sensor_msgs::PointCloud2 cloud2;
|
||||
|
||||
if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
|
||||
if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
|
||||
{
|
||||
printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
|
||||
return;
|
||||
|
|
@ -307,7 +307,7 @@ void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message,
|
|||
buffer->unlock();
|
||||
}
|
||||
|
||||
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& message,
|
||||
void ObstacleLayer::pointCloud2Callback(const robot_sensor_msgs::PointCloud2& message,
|
||||
const boost::shared_ptr<ObservationBuffer>& buffer)
|
||||
{
|
||||
// buffer the point cloud
|
||||
|
|
@ -346,13 +346,13 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
|
|||
{
|
||||
const Observation& obs = *it;
|
||||
|
||||
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
|
||||
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
|
||||
for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
|
|
@ -479,7 +479,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
|
|||
{
|
||||
double ox = clearing_observation.origin_.x;
|
||||
double oy = clearing_observation.origin_.y;
|
||||
const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
|
||||
const robot_sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);
|
||||
|
||||
// get the map coordinates of the origin of the sensor
|
||||
unsigned int x0, y0;
|
||||
|
|
@ -500,8 +500,8 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
|
|||
touch(ox, oy, min_x, min_y, max_x, max_y);
|
||||
|
||||
// for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
|
||||
for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -170,16 +170,16 @@ void StaticLayer::handleImpl(const void* data,
|
|||
const std::type_info& type,
|
||||
const std::string& topic)
|
||||
{
|
||||
if (type == typeid(nav_msgs::OccupancyGrid) && topic == "map") {
|
||||
incomingMap(*static_cast<const nav_msgs::OccupancyGrid*>(data));
|
||||
} else if (type == typeid(map_msgs::OccupancyGridUpdate) && topic == "map_update") {
|
||||
incomingUpdate(*static_cast<const map_msgs::OccupancyGridUpdate*>(data));
|
||||
if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == "map") {
|
||||
incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
|
||||
} else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == "map_update") {
|
||||
incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
|
||||
} else {
|
||||
std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
||||
void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
|
||||
{
|
||||
if(!map_shutdown_)
|
||||
{
|
||||
|
|
@ -248,7 +248,7 @@ void StaticLayer::incomingMap(const nav_msgs::OccupancyGrid& new_map)
|
|||
}
|
||||
}
|
||||
|
||||
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdate& update)
|
||||
void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& update)
|
||||
{
|
||||
if(!map_update_shutdown_)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
* David V. Lu!!
|
||||
*********************************************************************/
|
||||
#include <costmap_2d/voxel_layer.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <boost/dll/alias.hpp>
|
||||
|
||||
#define VOXEL_BITS 16
|
||||
|
|
@ -180,13 +180,13 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
|
|||
{
|
||||
const Observation& obs = *it;
|
||||
|
||||
const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
const robot_sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
|
||||
|
||||
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");
|
||||
|
||||
for (unsigned int i = 0; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
|
|
@ -333,9 +333,9 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
|
|||
double map_end_x = origin_x_ + getSizeInMetersX();
|
||||
double map_end_y = origin_y_ + getSizeInMetersY();
|
||||
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
|
||||
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
|
||||
robot_sensor_msgs::PointCloud2ConstIterator<float> iter_z(*(clearing_observation.cloud_), "z");
|
||||
|
||||
for (;iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@
|
|||
|
||||
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
#include <tf3_sensor_msgs/tf3_sensor_msgs.h>
|
||||
#include <sensor_msgs/point_cloud2_iterator.h>
|
||||
#include <robot_sensor_msgs/point_cloud2_iterator.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace tf3;
|
||||
|
|
@ -117,7 +117,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
|
|||
return true;
|
||||
}
|
||||
|
||||
void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
||||
void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
|
||||
{
|
||||
robot_geometry_msgs::PointStamped global_origin;
|
||||
|
||||
|
|
@ -154,7 +154,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
observation_list_.front().raytrace_range_ = raytrace_range_;
|
||||
observation_list_.front().obstacle_range_ = obstacle_range_;
|
||||
|
||||
sensor_msgs::PointCloud2 global_frame_cloud;
|
||||
robot_sensor_msgs::PointCloud2 global_frame_cloud;
|
||||
|
||||
// transform the point cloud
|
||||
// tf3_buffer_.transform(cloud, global_frame_cloud, global_frame_);
|
||||
|
|
@ -167,7 +167,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
global_frame_cloud.header.stamp = cloud.header.stamp;
|
||||
|
||||
// now we need to remove observations from the cloud that are below or above our height thresholds
|
||||
sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||
robot_sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
|
||||
observation_cloud.height = global_frame_cloud.height;
|
||||
observation_cloud.width = global_frame_cloud.width;
|
||||
observation_cloud.fields = global_frame_cloud.fields;
|
||||
|
|
@ -177,12 +177,12 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
|
|||
observation_cloud.is_dense = global_frame_cloud.is_dense;
|
||||
|
||||
unsigned int cloud_size = global_frame_cloud.height*global_frame_cloud.width;
|
||||
sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||
robot_sensor_msgs::PointCloud2Modifier modifier(observation_cloud);
|
||||
modifier.resize(cloud_size);
|
||||
unsigned int point_count = 0;
|
||||
|
||||
// copy over the points that are within our height bounds
|
||||
sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||
robot_sensor_msgs::PointCloud2Iterator<float> iter_z(global_frame_cloud, "z");
|
||||
std::vector<unsigned char>::const_iterator iter_global = global_frame_cloud.data.begin(), iter_global_end = global_frame_cloud.data.end();
|
||||
std::vector<unsigned char>::iterator iter_obs = observation_cloud.data.begin();
|
||||
for (; iter_global != iter_global_end; ++iter_z, iter_global += global_frame_cloud.point_step)
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user