diff --git a/CMakeLists.txt b/CMakeLists.txt index 339b498..be42065 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,8 @@ target_link_libraries(costmap_2d voxel_grid tf3 tf3_geometry_msgs + tf3_sensor_msgs + data_convert xmlrpcpp # XMLRPC ) diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index f633869..704f4da 100644 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include diff --git a/include/costmap_2d/data_convert.h b/include/costmap_2d/data_convert.h deleted file mode 100644 index 2604ad7..0000000 --- a/include/costmap_2d/data_convert.h +++ /dev/null @@ -1,159 +0,0 @@ -#ifndef COSTMAP_2D_DATA_CONVERT_H -#define COSTMAP_2D_DATA_CONVERT_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace costmap_2d -{ - template - bool isEqual(const T& a, const T& b, double eps = 1e-5) - { - return std::fabs(a - b) < eps; - } - - inline bool operator==(const geometry_msgs::Quaternion& a, const geometry_msgs::Quaternion& b) - { - return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z) && isEqual(a.w, b.w); - } - - inline bool operator==(const geometry_msgs::Point& a, const geometry_msgs::Point& b) - { - return isEqual(a.x, b.x) && isEqual(a.y, b.y) && isEqual(a.z, b.z); - } - - inline bool operator==(const geometry_msgs::Pose& a, const geometry_msgs::Pose& b) - { - return (a.position == b.position) && (a.orientation == b.orientation); - } - - inline bool operator==(const geometry_msgs::PoseStamped& a, const geometry_msgs::PoseStamped& b) - { - return (a.header.frame_id == b.header.frame_id) && - (a.pose == b.pose); - } - - inline double getYaw(const geometry_msgs::Quaternion& q) - { - double siny_cosp = 2.0 * (q.w * q.z + q.x * q.y); - double cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z); - return std::atan2(siny_cosp, cosy_cosp); - } - - inline geometry_msgs::Quaternion getQuaternion(const double& yaw) - { - geometry_msgs::Quaternion q; - q.x = 0.0; - q.y = 0.0; - q.z = std::sin(yaw / 2.0); - q.w = std::cos(yaw / 2.0); - return q; - } - - inline robot::Time convertTime(tf3::Time time) - { - robot::Time time_tmp; - time_tmp.sec = time.sec; - time_tmp.nsec = time.nsec; - return time_tmp; - } - - inline tf3::Time convertTime(robot::Time time) - { - tf3::Time time_tmp; - time_tmp.sec = time.sec; - time_tmp.nsec = time.nsec; - return time_tmp; - } - - inline tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q) - { - tf3::Quaternion out(q.x,q.y,q.z,q.w); - return out; - } - - inline geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q) - { - return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w()); - } - - inline tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg) - { - tf3::Quaternion q( - msg.rotation.x, - msg.rotation.y, - msg.rotation.z, - msg.rotation.w - ); - - tf3::Vector3 t( - msg.translation.x, - msg.translation.y, - msg.translation.z - ); - - tf3::Transform tf(q, t); - return tf; - } - - inline tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg) - { - tf3::Quaternion q( - msg.rotation.x, - msg.rotation.y, - msg.rotation.z, - msg.rotation.w - ); - - tf3::Vector3 t( - msg.translation.x, - msg.translation.y, - msg.translation.z - ); - - tf3::Transform tf(q, t); - return tf; - } - - inline tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg) - { - tf3::TransformStampedMsg out; - out.header.seq = msg.header.seq; - out.header.stamp = convertTime(msg.header.stamp); - out.header.frame_id = msg.header.frame_id; - out.child_frame_id = msg.child_frame_id; - out.transform.translation.x = msg.transform.translation.x; - out.transform.translation.y = msg.transform.translation.y; - out.transform.translation.z = msg.transform.translation.z; - out.transform.rotation.x = msg.transform.rotation.x; - out.transform.rotation.y = msg.transform.rotation.y; - out.transform.rotation.z = msg.transform.rotation.z; - out.transform.rotation.w = msg.transform.rotation.w; - return out; - } - - inline geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg) - { - geometry_msgs::TransformStamped out; - out.header.seq = msg.header.seq; - out.header.stamp = convertTime(msg.header.stamp); - out.header.frame_id = msg.header.frame_id; - out.child_frame_id = msg.child_frame_id; - out.transform.translation.x = msg.transform.translation.x; - out.transform.translation.y = msg.transform.translation.y; - out.transform.translation.z = msg.transform.translation.z; - out.transform.rotation.x = msg.transform.rotation.x; - out.transform.rotation.y = msg.transform.rotation.y; - out.transform.rotation.z = msg.transform.rotation.z; - out.transform.rotation.w = msg.transform.rotation.w; - return out; - } -} - -#endif // DATA_CONVERT_H \ No newline at end of file diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 757bd9c..1c5a534 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -1,7 +1,8 @@ #include -#include +#include #include #include +#include #include @@ -134,7 +135,7 @@ namespace costmap_2d return false; } // Convert to yaw - tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation); + tf3::Quaternion quaternion = data_convert::convertQuaternion(p.pose.orientation); double theta = tf3::getYaw(quaternion); unsigned char value = laneFilter(mx, my, theta); if (get_success) @@ -413,7 +414,7 @@ namespace costmap_2d } // Copy map data given proper transformations tf3::Transform tf3_transform; - tf3_transform = convertTotf3Transform(transformMsg.transform); + tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform); x = tf3_transform.getOrigin().x(); y = tf3_transform.getOrigin().y(); // Extract the rotation as a quaternion from the transform diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index f45958d..7826c62 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -1,7 +1,7 @@ #include #include -#include +#include #include #include #include @@ -9,6 +9,7 @@ #include #include #include +#include using costmap_2d::NO_INFORMATION; using costmap_2d::LETHAL_OBSTACLE; @@ -315,7 +316,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int } // Copy map data given proper transformations tf3::Transform tf3_transform; - tf3_transform = convertTotf3Transform(transformMsg.transform); + tf3_transform = data_convert::convertTotf3Transform(transformMsg.transform); // tf3::convert(transformMsg.transform, tf3_transform); for (unsigned int i = min_i; i < max_i; ++i) { diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 485c064..45d81c4 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -84,7 +84,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) } else { - std::cout << "/cfg folder not found!" << std::endl; + std::cout<< config_file_name << " file not found!" << std::endl; } @@ -102,8 +102,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) { if (last_error + robot::Duration(5.0) < robot::Time::now()) { - printf("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", - robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", + robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); last_error = robot::Time::now(); } // The error string will accumulate and errors will typically be the same, so the last @@ -293,7 +293,7 @@ void Costmap2DROBOT::updateMap() { double x = pose.pose.position.x, y = pose.pose.position.y, - yaw = getYaw(pose.pose.orientation); + yaw = data_convert::getYaw(pose.pose.orientation); layered_costmap_->updateMap(x, y, yaw); geometry_msgs::PolygonStamped footprint; footprint.header.frame_id = global_frame_; @@ -408,9 +408,9 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const try { // use current time if possible (makes sure it's not in the future) - if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time))) + if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time))) { - tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time)); + tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)); tf3::doTransform(robot_pose, global_pose, transform); } // use the latest otherwise @@ -420,7 +420,7 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const tf3::TransformStampedMsg transform = tf_.lookupTransform( global_frame_, // frame đích robot_base_frame_, // frame nguồn - tf3::convertTime(robot_pose.header.stamp) + data_convert::convertTime(robot_pose.header.stamp) ); tf3::doTransform(robot_pose, global_pose, transform); } @@ -460,7 +460,7 @@ void Costmap2DROBOT::getOrientedFootprint(std::vector& ori if (!getRobotPose(global_pose)) return; - double yaw = getYaw(global_pose.pose.orientation); + double yaw = data_convert::getYaw(global_pose.pose.orientation); transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw, padded_footprint_, oriented_footprint); } diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 061cc39..6b0fb7b 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -1,7 +1,7 @@ #include -#include -// #include -// #include +#include +#include +#include #include #include #include @@ -59,14 +59,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf3::doTransform(origin, origin, tf3_buffer_.lookupTransform(new_global_frame, origin.header.frame_id, - convertTime(origin.header.stamp))); + data_convert::convertTime(origin.header.stamp))); obs.origin_ = origin.point; // we also need to transform the cloud of the observation to the new global frame tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tf3_buffer_.lookupTransform(new_global_frame, obs.cloud_->header.frame_id, - convertTime(obs.cloud_->header.stamp))); + data_convert::convertTime(obs.cloud_->header.stamp))); } catch (TransformException& ex) { @@ -103,7 +103,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) tf3::doTransform(local_origin, global_origin, tf3_buffer_.lookupTransform(global_frame_, local_origin.header.frame_id, - convertTime(local_origin.header.stamp))); + data_convert::convertTime(local_origin.header.stamp))); tf3::convert(global_origin.point, observation_list_.front().origin_); // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations @@ -116,7 +116,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud) tf3::doTransform(cloud, global_frame_cloud, tf3_buffer_.lookupTransform(global_frame_, (cloud.header.frame_id), - convertTime(cloud.header.stamp))); + data_convert::convertTime(cloud.header.stamp))); 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