278 lines
9.1 KiB
C++
278 lines
9.1 KiB
C++
/*********************************************************************
|
|
*
|
|
* 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 COSTMAP_2D_COSTMAP_2D_ROBOT_H_
|
|
#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
|
|
|
|
#include <costmap_2d/layered_costmap.h>
|
|
#include <costmap_2d/layer.h>
|
|
#include <costmap_2d/footprint.h>
|
|
|
|
#include <geometry_msgs/Polygon.h>
|
|
#include <geometry_msgs/PolygonStamped.h>
|
|
#include <geometry_msgs/PoseStamped.h>
|
|
|
|
#include <tf3/LinearMath/Transform.h>
|
|
#include <robot/rate.h>
|
|
#include <data_convert/data_convert.h>
|
|
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
|
|
|
|
#include <xmlrpcpp/XmlRpcValue.h>
|
|
class SuperValue : public XmlRpc::XmlRpcValue
|
|
{
|
|
public:
|
|
void setStruct(XmlRpc::XmlRpcValue::ValueStruct* a)
|
|
{
|
|
_type = TypeStruct;
|
|
_value.asStruct = new XmlRpc::XmlRpcValue::ValueStruct(*a);
|
|
}
|
|
void setArray(XmlRpc::XmlRpcValue::ValueArray* a)
|
|
{
|
|
_type = TypeArray;
|
|
_value.asArray = new std::vector<XmlRpc::XmlRpcValue>(*a);
|
|
}
|
|
};
|
|
|
|
namespace costmap_2d
|
|
{
|
|
|
|
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
|
|
* topics that provide observations about obstacles in either the form
|
|
* of PointCloud or LaserScan messages. */
|
|
class Costmap2DROBOT
|
|
{
|
|
public:
|
|
/**
|
|
* @brief Constructor for the wrapper
|
|
* @param name The name for this costmap
|
|
* @param tf A reference to a TransformListener
|
|
*/
|
|
Costmap2DROBOT(const std::string &name, tf3::BufferCore& tf);
|
|
~Costmap2DROBOT();
|
|
|
|
/**
|
|
* @brief Subscribes to sensor topics if necessary and starts costmap
|
|
* updates, can be called to restart the costmap after calls to either
|
|
* stop() or pause()
|
|
*/
|
|
void start();
|
|
|
|
/**
|
|
* @brief Stops costmap updates and unsubscribes from sensor topics
|
|
*/
|
|
void stop();
|
|
|
|
/**
|
|
* @brief Stops the costmap from updating, but sensor data still comes in over the wire
|
|
*/
|
|
void pause();
|
|
|
|
/**
|
|
* @brief Resumes costmap updates
|
|
*/
|
|
void resume();
|
|
|
|
void updateMap();
|
|
|
|
/**
|
|
* @brief Reset each individual layer
|
|
*/
|
|
void resetLayers();
|
|
|
|
/** @brief Same as getLayeredCostmap()->isCurrent(). */
|
|
bool isCurrent() const
|
|
{
|
|
return layered_costmap_->isCurrent();
|
|
}
|
|
|
|
/**
|
|
* @brief Is the costmap stopped
|
|
*/
|
|
bool isStopped() const
|
|
{
|
|
return stopped_;
|
|
}
|
|
|
|
/**
|
|
* @brief Get the pose of the robot in the global frame of the costmap
|
|
* @param global_pose Will be set to the pose of the robot in the global frame of the costmap
|
|
* @return True if the pose was set successfully, false otherwise
|
|
*/
|
|
bool getRobotPose(geometry_msgs::PoseStamped& global_pose) const;
|
|
|
|
/** @brief Returns costmap name */
|
|
inline const std::string& getName() const noexcept
|
|
{
|
|
return name_;
|
|
}
|
|
|
|
/** @brief Returns the delay in transform (tf) data that is tolerable in seconds */
|
|
double getTransformTolerance() const
|
|
{
|
|
return transform_tolerance_;
|
|
}
|
|
|
|
/** @brief Return a pointer to the "master" costmap which receives updates from all the layers.
|
|
*
|
|
* Same as calling getLayeredCostmap()->getCostmap(). */
|
|
Costmap2D* getCostmap() const
|
|
{
|
|
return layered_costmap_->getCostmap();
|
|
}
|
|
|
|
/**
|
|
* @brief Returns the global frame of the costmap
|
|
* @return The global frame of the costmap
|
|
*/
|
|
inline const std::string& getGlobalFrameID() const noexcept
|
|
{
|
|
return global_frame_;
|
|
}
|
|
|
|
/**
|
|
* @brief Returns the local frame of the costmap
|
|
* @return The local frame of the costmap
|
|
*/
|
|
inline const std::string& getBaseFrameID() const noexcept
|
|
{
|
|
return robot_base_frame_;
|
|
}
|
|
LayeredCostmap* getLayeredCostmap() const
|
|
{
|
|
return layered_costmap_;
|
|
}
|
|
|
|
/** @brief Returns the current padded footprint as a geometry_msgs::Polygon. */
|
|
geometry_msgs::Polygon getRobotFootprintPolygon() const
|
|
{
|
|
return costmap_2d::toPolygon(padded_footprint_);
|
|
}
|
|
|
|
/** @brief Return the current footprint of the robot as a vector of points.
|
|
*
|
|
* This version of the footprint is padded by the footprint_padding_
|
|
* distance, set in the rosparam "footprint_padding".
|
|
*
|
|
* The footprint initially comes from the rosparam "footprint" but
|
|
* can be overwritten by dynamic reconfigure or by messages received
|
|
* on the "footprint" topic. */
|
|
inline const std::vector<geometry_msgs::Point>& getRobotFootprint() const noexcept
|
|
{
|
|
return padded_footprint_;
|
|
}
|
|
|
|
/** @brief Return the current unpadded footprint of the robot as a vector of points.
|
|
*
|
|
* This is the raw version of the footprint without padding.
|
|
*
|
|
* The footprint initially comes from the rosparam "footprint" but
|
|
* can be overwritten by dynamic reconfigure or by messages received
|
|
* on the "footprint" topic. */
|
|
inline const std::vector<geometry_msgs::Point>& getUnpaddedRobotFootprint() const noexcept
|
|
{
|
|
return unpadded_footprint_;
|
|
}
|
|
|
|
/**
|
|
* @brief Build the oriented footprint of the robot at the robot's current pose
|
|
* @param oriented_footprint Will be filled with the points in the oriented footprint of the robot
|
|
*/
|
|
void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const;
|
|
|
|
/** @brief Set the footprint of the robot to be the given set of
|
|
* points, padded by footprint_padding.
|
|
*
|
|
* Should be a convex polygon, though this is not enforced.
|
|
*
|
|
* First expands the given polygon by footprint_padding_ and then
|
|
* sets padded_footprint_ and calls
|
|
* layered_costmap_->setFootprint(). Also saves the unpadded
|
|
* footprint, which is available from
|
|
* getUnpaddedRobotFootprint(). */
|
|
void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points);
|
|
|
|
/** @brief Set the footprint of the robot to be the given polygon,
|
|
* padded by footprint_padding.
|
|
*
|
|
* Should be a convex polygon, though this is not enforced.
|
|
*
|
|
* First expands the given polygon by footprint_padding_ and then
|
|
* sets padded_footprint_ and calls
|
|
* layered_costmap_->setFootprint(). Also saves the unpadded
|
|
* footprint, which is available from
|
|
* getUnpaddedRobotFootprint(). */
|
|
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint);
|
|
|
|
protected:
|
|
LayeredCostmap* layered_costmap_;
|
|
std::string name_;
|
|
tf3::BufferCore& tf_; ///< @brief Used for transforming point clouds
|
|
std::string global_frame_; ///< @brief The global frame for the costmap
|
|
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
|
|
double transform_tolerance_; ///< timeout before transform errors
|
|
|
|
private:
|
|
/** @brief Set the footprint from the new_config object.
|
|
*
|
|
* If the values of footprint and robot_radius are the same in
|
|
* new_config and old_config, nothing is changed. */
|
|
void readFootprintFromConfig(const std::vector<geometry_msgs::Point> &new_footprint,
|
|
const std::vector<geometry_msgs::Point> &old_footprint,
|
|
const double &robot_radius);
|
|
|
|
void checkMovement();
|
|
void mapUpdateLoop(double frequency);
|
|
bool map_update_thread_shutdown_;
|
|
bool stop_updates_, initialized_, stopped_;
|
|
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
|
|
robot::Time last_publish_;
|
|
robot::Duration publish_cycle;
|
|
|
|
boost::recursive_mutex configuration_mutex_;
|
|
|
|
std::vector<geometry_msgs::Point> unpadded_footprint_;
|
|
std::vector<geometry_msgs::Point> padded_footprint_;
|
|
float footprint_padding_;
|
|
|
|
private:
|
|
void getParams(const std::string& config_file_name);
|
|
};
|
|
// class Costmap2DROBOT
|
|
} // namespace costmap_2d
|
|
|
|
#endif // COSTMAP_2D_COSTMAP_2D_ROBOT_H
|