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

update file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd
2025-11-26 15:00:26 +07:00
parent 19a96c27d5
commit 2e0a4348dd
9 changed files with 551 additions and 1200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
include/costmap_2d/costmap_2d_robot.h
View File

@@ -41,27 +41,31 @@
#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 <costmap_2d/data_convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.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);
// }
// };
#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
{
@@ -242,40 +246,30 @@ protected:
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 costmap_2d::Costmap2DConfig &new_config,
// const costmap_2d::Costmap2DConfig &old_config);
/** @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 loadOldParameters(ros::NodeHandle& nh);
// void warnForOldParameters(ros::NodeHandle& nh);
// void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
// void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
// void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
// void movementCB(const ros::TimerEvent &event);
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;
// pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
// Costmap2DPublisher* publisher_;
// dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_;
// ros::Subscriber footprint_sub_;
// ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
// costmap_2d::Costmap2DConfig old_config_;
private:
void getParams();
void getParams(const std::string& config_file_name);
};
// class Costmap2DROBOT
} // namespace costmap_2d

21
include/costmap_2d/data_convert.h
View File

@@ -1,5 +1,5 @@
#ifndef DATA_CONVERT_H
#define DATA_CONVERT_H
#ifndef COSTMAP_2D_DATA_CONVERT_H
#define COSTMAP_2D_DATA_CONVERT_H
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
@@ -56,7 +56,7 @@ namespace costmap_2d
return q;
}
robot::Time convertTime(tf3::Time time)
inline robot::Time convertTime(tf3::Time time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
@@ -64,7 +64,7 @@ namespace costmap_2d
return time_tmp;
}
tf3::Time convertTime(robot::Time time)
inline tf3::Time convertTime(robot::Time time)
{
tf3::Time time_tmp;
time_tmp.sec = time.sec;
@@ -72,18 +72,18 @@ namespace costmap_2d
return time_tmp;
}
tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
{
tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out;
}
geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
}
tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
{
tf3::Quaternion q(
msg.rotation.x,
@@ -102,7 +102,7 @@ namespace costmap_2d
return tf;
}
tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
{
tf3::Quaternion q(
msg.rotation.x,
@@ -121,7 +121,7 @@ namespace costmap_2d
return tf;
}
tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
{
tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq;
@@ -137,7 +137,8 @@ namespace costmap_2d
out.transform.rotation.w = msg.transform.rotation.w;
return out;
}
geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{
geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq;

46
include/costmap_2d/footprint.h
View File

@@ -41,6 +41,8 @@
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include<string.h>
#include<vector>
@@ -134,30 +136,30 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ge
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
// /**
// * @brief Read the ros-params "footprint" and/or "robot_radius" from
// * the given NodeHandle using searchParam() to go up the tree.
// */
// std::vector<geometry_msgs::Point> makeFootprintFromParams(std::string& nh);
/**
* @brief Read the ros-params "footprint" and/or "robot_radius" from
* the given NodeHandle using searchParam() to go up the tree.
*/
std::vector<geometry_msgs::Point> makeFootprintFromParams(const std::string& file_name);
// /**
// * @brief Create the footprint from the given XmlRpcValue.
// *
// * @param footprint_xmlrpc should be an array of arrays, where the
// * top-level array should have 3 or more elements, and the
// * sub-arrays should all have exactly 2 elements (x and y
// * coordinates).
// *
// * @param full_param_name this is the full name of the rosparam from
// * which the footprint_xmlrpc value came. It is used only for
// * reporting errors. */
// std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpcValue& footprint_xmlrpc,
// const std::string& full_param_name);
/**
* @brief Create the footprint from the given XmlRpcValue.
*
* @param footprint_xmlrpc should be an array of arrays, where the
* top-level array should have 3 or more elements, and the
* sub-arrays should all have exactly 2 elements (x and y
* coordinates).
*
* @param full_param_name this is the full name of the rosparam from
* which the footprint_xmlrpc value came. It is used only for
* reporting errors. */
std::vector<geometry_msgs::Point> makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc,
const std::string& full_param_name);
// /** @brief Write the current unpadded_footprint_ to the "footprint"
// * parameter of the given NodeHandle so that dynamic_reconfigure
// * will see the new value. */
// void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
/** @brief Write the current unpadded_footprint_ to the "footprint"
* parameter of the given NodeHandle so that dynamic_reconfigure
* will see the new value. */
void writeFootprintToParam(std::string& nh, const std::vector<geometry_msgs::Point>& footprint);
} // end namespace costmap_2d