Hiep update
This commit is contained in:
280
include/robot_costmap_2d/costmap_2d_robot.h
Executable file
280
include/robot_costmap_2d/costmap_2d_robot.h
Executable file
@@ -0,0 +1,280 @@
|
||||
/*********************************************************************
|
||||
*
|
||||
* 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 ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
|
||||
#define ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H_
|
||||
|
||||
#include <robot_costmap_2d/layered_costmap.h>
|
||||
#include <robot_costmap_2d/layer.h>
|
||||
#include <robot_costmap_2d/footprint.h>
|
||||
|
||||
#include <robot_geometry_msgs/Polygon.h>
|
||||
#include <robot_geometry_msgs/PolygonStamped.h>
|
||||
#include <robot_geometry_msgs/PoseStamped.h>
|
||||
|
||||
#include <tf3/LinearMath/Transform.h>
|
||||
#include <robot/rate.h>
|
||||
#include <data_convert/data_convert.h>
|
||||
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
|
||||
|
||||
#include <robot_xmlrpcpp/XmlRpcValue.h>
|
||||
|
||||
#include <robot/node_handle.h>
|
||||
#include <robot/plugin_loader_helper.h>
|
||||
class SuperValue : public robot::XmlRpc::XmlRpcValue
|
||||
{
|
||||
public:
|
||||
void setStruct(robot::XmlRpc::XmlRpcValue::ValueStruct* a)
|
||||
{
|
||||
_type = TypeStruct;
|
||||
_value.asStruct = new robot::XmlRpc::XmlRpcValue::ValueStruct(*a);
|
||||
}
|
||||
void setArray(robot::XmlRpc::XmlRpcValue::ValueArray* a)
|
||||
{
|
||||
_type = TypeArray;
|
||||
_value.asArray = new std::vector<robot::XmlRpc::XmlRpcValue>(*a);
|
||||
}
|
||||
};
|
||||
|
||||
namespace robot_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(robot_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 robot_geometry_msgs::Polygon. */
|
||||
robot_geometry_msgs::Polygon getRobotFootprintPolygon() const
|
||||
{
|
||||
return robot_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<robot_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<robot_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<robot_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<robot_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 robot_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<robot_geometry_msgs::Point> &new_footprint,
|
||||
const std::vector<robot_geometry_msgs::Point> &old_footprint,
|
||||
const double &robot_radius);
|
||||
std::vector<boost::function<PluginLayerPtr()>> creators_;
|
||||
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<robot_geometry_msgs::Point> unpadded_footprint_;
|
||||
std::vector<robot_geometry_msgs::Point> padded_footprint_;
|
||||
float footprint_padding_;
|
||||
|
||||
private:
|
||||
void getParams(const std::string& config_file_name, robot::NodeHandle& nh);
|
||||
};
|
||||
// class Costmap2DROBOT
|
||||
} // namespace robot_costmap_2d
|
||||
|
||||
#endif // ROBOT_COSTMAP_2D_ROBOT_COSTMAP_2D_ROBOT_H
|
||||
Reference in New Issue
Block a user