mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
Added modified static_layer costmap_2d plugin
This commit is contained in:
Mathieu Labbe
@@ -0,0 +1,266 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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!!
|
||||
*********************************************************************/
|
||||
|
||||
/*
|
||||
* Modified: added "layered_costmap_->updateMap(0,0,0);" below
|
||||
*/
|
||||
|
||||
#include "static_layer.h"
|
||||
#include <costmap_2d/costmap_math.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StaticLayer, costmap_2d::Layer)
|
||||
|
||||
using costmap_2d::NO_INFORMATION;
|
||||
using costmap_2d::LETHAL_OBSTACLE;
|
||||
using costmap_2d::FREE_SPACE;
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
StaticLayer::StaticLayer() : dsrv_(NULL) {}
|
||||
|
||||
StaticLayer::~StaticLayer()
|
||||
{
|
||||
if(dsrv_)
|
||||
delete dsrv_;
|
||||
}
|
||||
|
||||
void StaticLayer::onInitialize()
|
||||
{
|
||||
ros::NodeHandle nh("~/" + name_), g_nh;
|
||||
current_ = true;
|
||||
|
||||
global_frame_ = layered_costmap_->getGlobalFrameID();
|
||||
|
||||
std::string map_topic;
|
||||
nh.param("map_topic", map_topic, std::string("map"));
|
||||
nh.param("subscribe_to_updates", subscribe_to_updates_, false);
|
||||
|
||||
nh.param("track_unknown_space", track_unknown_space_, true);
|
||||
nh.param("use_maximum", use_maximum_, false);
|
||||
|
||||
int temp_lethal_threshold, temp_unknown_cost_value;
|
||||
nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
|
||||
nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
|
||||
nh.param("trinary_costmap", trinary_costmap_, true);
|
||||
|
||||
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
|
||||
unknown_cost_value_ = temp_unknown_cost_value;
|
||||
//we'll subscribe to the latched topic that the map server uses
|
||||
ROS_INFO("Requesting the map...");
|
||||
map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
|
||||
map_received_ = false;
|
||||
has_updated_data_ = false;
|
||||
|
||||
ros::Rate r(10);
|
||||
while (!map_received_ && g_nh.ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
|
||||
|
||||
if(subscribe_to_updates_)
|
||||
{
|
||||
ROS_INFO("Subscribing to updates");
|
||||
map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
|
||||
}
|
||||
|
||||
if(dsrv_)
|
||||
{
|
||||
delete dsrv_;
|
||||
}
|
||||
|
||||
dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
|
||||
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
|
||||
&StaticLayer::reconfigureCB, this, _1, _2);
|
||||
dsrv_->setCallback(cb);
|
||||
}
|
||||
|
||||
void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
|
||||
{
|
||||
if (config.enabled != enabled_)
|
||||
{
|
||||
enabled_ = config.enabled;
|
||||
has_updated_data_ = true;
|
||||
x_ = y_ = 0;
|
||||
width_ = size_x_;
|
||||
height_ = size_y_;
|
||||
}
|
||||
}
|
||||
|
||||
void StaticLayer::matchSize()
|
||||
{
|
||||
Costmap2D* master = layered_costmap_->getCostmap();
|
||||
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
|
||||
master->getOriginX(), master->getOriginY());
|
||||
}
|
||||
|
||||
unsigned char StaticLayer::interpretValue(unsigned char value)
|
||||
{
|
||||
//check if the static value is above the unknown or lethal thresholds
|
||||
if (track_unknown_space_ && value == unknown_cost_value_)
|
||||
return NO_INFORMATION;
|
||||
else if (value >= lethal_threshold_)
|
||||
return LETHAL_OBSTACLE;
|
||||
else if (trinary_costmap_)
|
||||
return FREE_SPACE;
|
||||
|
||||
double scale = (double) value / lethal_threshold_;
|
||||
return scale * LETHAL_OBSTACLE;
|
||||
}
|
||||
|
||||
void StaticLayer::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, size_y, new_map->info.resolution);
|
||||
|
||||
// resize costmap if size, resolution or origin do not match
|
||||
Costmap2D* master = layered_costmap_->getCostmap();
|
||||
if (master->getSizeInCellsX() != size_x ||
|
||||
master->getSizeInCellsY() != size_y ||
|
||||
master->getResolution() != new_map->info.resolution ||
|
||||
master->getOriginX() != new_map->info.origin.position.x ||
|
||||
master->getOriginY() != new_map->info.origin.position.y ||
|
||||
!layered_costmap_->isSizeLocked())
|
||||
{
|
||||
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
|
||||
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
|
||||
new_map->info.origin.position.y, true);
|
||||
}else if(size_x_ != size_x || size_y_ != size_y ||
|
||||
resolution_ != new_map->info.resolution ||
|
||||
origin_x_ != new_map->info.origin.position.x ||
|
||||
origin_y_ != new_map->info.origin.position.y){
|
||||
matchSize();
|
||||
}
|
||||
|
||||
unsigned int index = 0;
|
||||
|
||||
//initialize the costmap with static data
|
||||
for (unsigned int i = 0; i < size_y; ++i)
|
||||
{
|
||||
for (unsigned int j = 0; j < size_x; ++j)
|
||||
{
|
||||
unsigned char value = new_map->data[index];
|
||||
costmap_[index] = interpretValue(value);
|
||||
++index;
|
||||
}
|
||||
}
|
||||
x_ = y_ = 0;
|
||||
width_ = size_x_;
|
||||
height_ = size_y_;
|
||||
map_received_ = true;
|
||||
has_updated_data_ = true;
|
||||
|
||||
layered_costmap_->updateMap(0,0,0);
|
||||
}
|
||||
|
||||
void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
|
||||
{
|
||||
unsigned int di = 0;
|
||||
for (unsigned int y = 0; y < update->height ; y++)
|
||||
{
|
||||
unsigned int index_base = (update->y + y) * size_x_;
|
||||
for (unsigned int x = 0; x < update->width ; x++)
|
||||
{
|
||||
unsigned int index = index_base + x + update->x;
|
||||
costmap_[index] = interpretValue( update->data[di++] );
|
||||
}
|
||||
}
|
||||
x_ = update->x;
|
||||
y_ = update->y;
|
||||
width_ = update->width;
|
||||
height_ = update->height;
|
||||
has_updated_data_ = true;
|
||||
|
||||
layered_costmap_->updateMap(0,0,0);
|
||||
}
|
||||
|
||||
void StaticLayer::activate()
|
||||
{
|
||||
onInitialize();
|
||||
}
|
||||
|
||||
void StaticLayer::deactivate()
|
||||
{
|
||||
map_sub_.shutdown();
|
||||
if (subscribe_to_updates_)
|
||||
map_update_sub_.shutdown();
|
||||
}
|
||||
|
||||
void StaticLayer::reset()
|
||||
{
|
||||
deactivate();
|
||||
activate();
|
||||
}
|
||||
|
||||
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
|
||||
double* max_x, double* max_y)
|
||||
{
|
||||
if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
|
||||
return;
|
||||
|
||||
useExtraBounds(min_x, min_y, max_x, max_y);
|
||||
|
||||
double mx, my;
|
||||
|
||||
mapToWorld(x_, y_, mx, my);
|
||||
*min_x = std::min(mx, *min_x);
|
||||
*min_y = std::min(my, *min_y);
|
||||
|
||||
mapToWorld(x_ + width_, y_ + height_, mx, my);
|
||||
*max_x = std::max(mx, *max_x);
|
||||
*max_y = std::max(my, *max_y);
|
||||
|
||||
has_updated_data_ = false;
|
||||
}
|
||||
|
||||
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
|
||||
{
|
||||
if (!map_received_)
|
||||
return;
|
||||
if (!use_maximum_)
|
||||
updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
|
||||
else
|
||||
updateWithMax(master_grid, min_i, min_j, max_i, max_j);
|
||||
}
|
||||
|
||||
} // namespace rtabmap_ros
|
||||
@@ -0,0 +1,100 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 RTABMAP_ROS_STATIC_LAYER_H_
|
||||
#define RTABMAP_ROS_STATIC_LAYER_H_
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <costmap_2d/costmap_layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_2d/GenericPluginConfig.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
#include <nav_msgs/OccupancyGrid.h>
|
||||
#include <map_msgs/OccupancyGridUpdate.h>
|
||||
#include <message_filters/subscriber.h>
|
||||
|
||||
namespace rtabmap_ros
|
||||
{
|
||||
|
||||
class StaticLayer : public costmap_2d::CostmapLayer
|
||||
{
|
||||
public:
|
||||
StaticLayer();
|
||||
virtual ~StaticLayer();
|
||||
virtual void onInitialize();
|
||||
virtual void activate();
|
||||
virtual void deactivate();
|
||||
virtual void reset();
|
||||
|
||||
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x,
|
||||
double* max_y);
|
||||
virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
|
||||
|
||||
virtual void matchSize();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Callback to update the costmap's map from the map_server
|
||||
* @param new_map The map to put into the costmap. The origin of the new
|
||||
* map along with its size will determine what parts of the costmap's
|
||||
* static map are overwritten.
|
||||
*/
|
||||
void incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map);
|
||||
void incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update);
|
||||
void reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level);
|
||||
|
||||
unsigned char interpretValue(unsigned char value);
|
||||
|
||||
std::string global_frame_; ///< @brief The global frame for the costmap
|
||||
bool subscribe_to_updates_;
|
||||
bool map_received_;
|
||||
bool has_updated_data_;
|
||||
unsigned int x_,y_,width_,height_;
|
||||
bool track_unknown_space_;
|
||||
bool use_maximum_;
|
||||
bool trinary_costmap_;
|
||||
ros::Subscriber map_sub_, map_update_sub_;
|
||||
|
||||
unsigned char lethal_threshold_, unknown_cost_value_;
|
||||
|
||||
mutable boost::recursive_mutex lock_;
|
||||
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
|
||||
};
|
||||
|
||||
} // namespace rtabmap_ros
|
||||
|
||||
#endif // RTABMAP_ROS_DYNAMIC_LAYER_H_
|
||||
Reference in New Issue
Block a user