add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,82 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav_2d_utils)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(nav2_common REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(nav_2d_msgs REQUIRED)
|
||||
find_package(nav_msgs REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
find_package(nav2_msgs REQUIRED)
|
||||
find_package(nav2_util REQUIRED)
|
||||
|
||||
nav2_package()
|
||||
|
||||
set(dependencies
|
||||
geometry_msgs
|
||||
nav_2d_msgs
|
||||
nav_msgs
|
||||
std_msgs
|
||||
rclcpp
|
||||
tf2
|
||||
tf2_geometry_msgs
|
||||
nav2_msgs
|
||||
nav2_util
|
||||
nav_2d_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
add_library(conversions SHARED
|
||||
src/conversions.cpp)
|
||||
|
||||
ament_target_dependencies(conversions
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
add_library(path_ops SHARED
|
||||
src/path_ops.cpp)
|
||||
|
||||
ament_target_dependencies(path_ops
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
add_library(tf_help SHARED
|
||||
src/tf_help.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(tf_help
|
||||
${dependencies}
|
||||
)
|
||||
|
||||
target_link_libraries(tf_help conversions)
|
||||
|
||||
install(TARGETS conversions path_ops tf_help
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(conversions path_ops tf_help)
|
||||
ament_export_dependencies(${dependencies})
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,71 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS__CONVERSIONS_HPP_
|
||||
#define NAV_2D_UTILS__CONVERSIONS_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "nav_2d_msgs/msg/twist2_d.hpp"
|
||||
#include "nav_2d_msgs/msg/path2_d.hpp"
|
||||
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "tf2/convert.h"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d);
|
||||
nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel);
|
||||
// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(const tf2::Stamped<tf2::Pose>& pose);
|
||||
nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(const geometry_msgs::msg::PoseStamped & pose);
|
||||
geometry_msgs::msg::Pose2D poseToPose2D(const geometry_msgs::msg::Pose & pose);
|
||||
geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d);
|
||||
geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
|
||||
const nav_2d_msgs::msg::Pose2DStamped & pose2d);
|
||||
geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
|
||||
const geometry_msgs::msg::Pose2D & pose2d,
|
||||
const std::string & frame, const rclcpp::Time & stamp);
|
||||
nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses);
|
||||
nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path);
|
||||
nav_msgs::msg::Path poses2DToPath(
|
||||
const std::vector<geometry_msgs::msg::Pose2D> & poses,
|
||||
const std::string & frame, const rclcpp::Time & stamp);
|
||||
nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d);
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS__CONVERSIONS_HPP_
|
||||
+101
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
|
||||
#define NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
|
||||
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include "nav_2d_msgs/msg/twist2_d_stamped.hpp"
|
||||
#include "nav_msgs/msg/odometry.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
|
||||
/**
|
||||
* @class OdomSubscriber
|
||||
* Wrapper for some common odometry operations. Subscribes to the topic with a mutex.
|
||||
*/
|
||||
class OdomSubscriber
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor that subscribes to an Odometry topic
|
||||
*
|
||||
* @param nh NodeHandle for creating subscriber
|
||||
* @param default_topic Name of the topic that will be loaded of the odom_topic param is not set.
|
||||
*/
|
||||
explicit OdomSubscriber(
|
||||
nav2_util::LifecycleNode::SharedPtr nh,
|
||||
std::string default_topic = "odom")
|
||||
{
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh, "odom_topic", rclcpp::ParameterValue(default_topic));
|
||||
|
||||
std::string odom_topic;
|
||||
nh->get_parameter_or("odom_topic", odom_topic, default_topic);
|
||||
odom_sub_ =
|
||||
nh->create_subscription<nav_msgs::msg::Odometry>(
|
||||
odom_topic,
|
||||
rclcpp::SystemDefaultsQoS(),
|
||||
std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;}
|
||||
inline nav_2d_msgs::msg::Twist2DStamped getTwistStamped() {return odom_vel_;}
|
||||
|
||||
protected:
|
||||
void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
|
||||
{
|
||||
// ROS_INFO_ONCE("odom received!");
|
||||
std::lock_guard<std::mutex> lock(odom_mutex_);
|
||||
odom_vel_.header = msg->header;
|
||||
odom_vel_.velocity.x = msg->twist.twist.linear.x;
|
||||
odom_vel_.velocity.y = msg->twist.twist.linear.y;
|
||||
odom_vel_.velocity.theta = msg->twist.twist.angular.z;
|
||||
}
|
||||
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
||||
nav_2d_msgs::msg::Twist2DStamped odom_vel_;
|
||||
std::mutex odom_mutex_;
|
||||
};
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS__ODOM_SUBSCRIBER_HPP_
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS__PARAMETERS_HPP_
|
||||
#define NAV_2D_UTILS__PARAMETERS_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_util/lifecycle_node.hpp"
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
|
||||
// TODO(crdelsey): Remove when code is re-enabled
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-parameter"
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Search for a parameter and load it, or use the default value
|
||||
*
|
||||
* This templated function shortens a commonly used ROS pattern in which you
|
||||
* search for a parameter and get its value if it exists, otherwise returning a default value.
|
||||
*
|
||||
* @param nh NodeHandle to start the parameter search from
|
||||
* @param param_name Name of the parameter to search for
|
||||
* @param default_value Value to return if not found
|
||||
* @return Value of parameter if found, otherwise the default_value
|
||||
*/
|
||||
template<class param_t>
|
||||
param_t searchAndGetParam(
|
||||
const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & param_name,
|
||||
const param_t & default_value)
|
||||
{
|
||||
param_t value;
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
nh, param_name,
|
||||
rclcpp::ParameterValue(default_value));
|
||||
nh->get_parameter(param_name, value);
|
||||
return value;
|
||||
}
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
#endif // NAV_2D_UTILS__PARAMETERS_HPP_
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS__PATH_OPS_HPP_
|
||||
#define NAV_2D_UTILS__PATH_OPS_HPP_
|
||||
|
||||
#include "nav_2d_msgs/msg/path2_d.hpp"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
/**
|
||||
* @brief Increase plan resolution to match that of the costmap by adding points linearly between points
|
||||
*
|
||||
* @param global_plan_in input plan
|
||||
* @param resolution desired distance between waypoints
|
||||
* @return Higher resolution plan
|
||||
*/
|
||||
nav_2d_msgs::msg::Path2D adjustPlanResolution(
|
||||
const nav_2d_msgs::msg::Path2D & global_plan_in,
|
||||
double resolution);
|
||||
} // namespace nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS__PATH_OPS_HPP_
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#ifndef NAV_2D_UTILS__TF_HELP_HPP_
|
||||
#define NAV_2D_UTILS__TF_HELP_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav_2d_msgs/msg/pose2_d_stamped.hpp"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
/**
|
||||
* @brief Transform a PoseStamped from one frame to another while catching exceptions
|
||||
*
|
||||
* Also returns immediately if the frames are equal.
|
||||
* @param tf Smart pointer to TFListener
|
||||
* @param frame Frame to transform the pose into
|
||||
* @param in_pose Pose to transform
|
||||
* @param out_pose Place to store the resulting transformed pose
|
||||
* @return True if successful transform
|
||||
*/
|
||||
bool transformPose(
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::string frame,
|
||||
const geometry_msgs::msg::PoseStamped & in_pose,
|
||||
geometry_msgs::msg::PoseStamped & out_pose,
|
||||
rclcpp::Duration & transform_tolerance
|
||||
);
|
||||
|
||||
/**
|
||||
* @brief Transform a Pose2DStamped from one frame to another while catching exceptions
|
||||
*
|
||||
* Also returns immediately if the frames are equal.
|
||||
* @param tf Smart pointer to TFListener
|
||||
* @param frame Frame to transform the pose into
|
||||
* @param in_pose Pose to transform
|
||||
* @param out_pose Place to store the resulting transformed pose
|
||||
* @return True if successful transform
|
||||
*/
|
||||
bool transformPose(
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::string frame,
|
||||
const nav_2d_msgs::msg::Pose2DStamped & in_pose,
|
||||
nav_2d_msgs::msg::Pose2DStamped & out_pose,
|
||||
rclcpp::Duration & transform_tolerance
|
||||
);
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
|
||||
#endif // NAV_2D_UTILS__TF_HELP_HPP_
|
||||
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav_2d_utils</name>
|
||||
<version>1.1.18</version>
|
||||
<description>A handful of useful utility functions for nav_2d packages.</description>
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<license>BSD-3-Clause</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>nav2_common</build_depend>
|
||||
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_2d_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>nav2_util</depend>
|
||||
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,187 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include "geometry_msgs/msg/pose2_d.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wpedantic"
|
||||
#include "tf2/utils.h"
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
using nav2_util::geometry_utils::orientationAroundZAxis;
|
||||
|
||||
geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d)
|
||||
{
|
||||
geometry_msgs::msg::Twist cmd_vel;
|
||||
cmd_vel.linear.x = cmd_vel_2d.x;
|
||||
cmd_vel.linear.y = cmd_vel_2d.y;
|
||||
cmd_vel.angular.z = cmd_vel_2d.theta;
|
||||
return cmd_vel;
|
||||
}
|
||||
|
||||
nav_2d_msgs::msg::Twist2D twist3Dto2D(const geometry_msgs::msg::Twist & cmd_vel)
|
||||
{
|
||||
nav_2d_msgs::msg::Twist2D cmd_vel_2d;
|
||||
cmd_vel_2d.x = cmd_vel.linear.x;
|
||||
cmd_vel_2d.y = cmd_vel.linear.y;
|
||||
cmd_vel_2d.theta = cmd_vel.angular.z;
|
||||
return cmd_vel_2d;
|
||||
}
|
||||
|
||||
// nav_2d_msgs::msg::Pose2DStamped stampedPoseToPose2D(const tf2::Stamped<tf2::Pose>& pose)
|
||||
// {
|
||||
// nav_2d_msgs::msg::Pose2DStamped pose2d;
|
||||
// pose2d.header.stamp = pose.stamp_;
|
||||
// pose2d.header.frame_id = pose.frame_id_;
|
||||
// pose2d.pose.x = pose.getOrigin().getX();
|
||||
// pose2d.pose.y = pose.getOrigin().getY();
|
||||
// pose2d.pose.theta = tf::getYaw(pose.getRotation());
|
||||
// return pose2d;
|
||||
// }
|
||||
|
||||
nav_2d_msgs::msg::Pose2DStamped poseStampedToPose2D(const geometry_msgs::msg::PoseStamped & pose)
|
||||
{
|
||||
nav_2d_msgs::msg::Pose2DStamped pose2d;
|
||||
pose2d.header = pose.header;
|
||||
pose2d.pose.x = pose.pose.position.x;
|
||||
pose2d.pose.y = pose.pose.position.y;
|
||||
pose2d.pose.theta = tf2::getYaw(pose.pose.orientation);
|
||||
return pose2d;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose2D poseToPose2D(const geometry_msgs::msg::Pose & pose)
|
||||
{
|
||||
geometry_msgs::msg::Pose2D pose2d;
|
||||
pose2d.x = pose.position.x;
|
||||
pose2d.y = pose.position.y;
|
||||
pose2d.theta = tf2::getYaw(pose.orientation);
|
||||
return pose2d;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d)
|
||||
{
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position.x = pose2d.x;
|
||||
pose.position.y = pose2d.y;
|
||||
pose.orientation = orientationAroundZAxis(pose2d.theta);
|
||||
return pose;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
|
||||
const nav_2d_msgs::msg::Pose2DStamped & pose2d)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = pose2d.header;
|
||||
pose.pose = pose2DToPose(pose2d.pose);
|
||||
return pose;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
|
||||
const geometry_msgs::msg::Pose2D & pose2d,
|
||||
const std::string & frame, const rclcpp::Time & stamp)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header.frame_id = frame;
|
||||
pose.header.stamp = stamp;
|
||||
pose.pose.position.x = pose2d.x;
|
||||
pose.pose.position.y = pose2d.y;
|
||||
pose.pose.orientation = orientationAroundZAxis(pose2d.theta);
|
||||
return pose;
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path posesToPath(const std::vector<geometry_msgs::msg::PoseStamped> & poses)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
if (poses.empty()) {
|
||||
return path;
|
||||
}
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = poses[0].header.frame_id;
|
||||
path.header.stamp = poses[0].header.stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++) {
|
||||
path.poses[i] = poses[i];
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
nav_2d_msgs::msg::Path2D pathToPath2D(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D path2d;
|
||||
path2d.header = path.header;
|
||||
for (auto & pose : path.poses) {
|
||||
path2d.poses.push_back(poseToPose2D(pose.pose));
|
||||
}
|
||||
return path2d;
|
||||
}
|
||||
|
||||
|
||||
nav_msgs::msg::Path poses2DToPath(
|
||||
const std::vector<geometry_msgs::msg::Pose2D> & poses,
|
||||
const std::string & frame, const rclcpp::Time & stamp)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
path.poses.resize(poses.size());
|
||||
path.header.frame_id = frame;
|
||||
path.header.stamp = stamp;
|
||||
for (unsigned int i = 0; i < poses.size(); i++) {
|
||||
path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path pathToPath(const nav_2d_msgs::msg::Path2D & path2d)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
path.header = path2d.header;
|
||||
path.poses.resize(path2d.poses.size());
|
||||
for (unsigned int i = 0; i < path.poses.size(); i++) {
|
||||
path.poses[i].header = path2d.header;
|
||||
path.poses[i].pose = pose2DToPose(path2d.poses[i]);
|
||||
}
|
||||
return path;
|
||||
}
|
||||
|
||||
} // namespace nav_2d_utils
|
||||
@@ -0,0 +1,84 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
#include <cmath>
|
||||
|
||||
using std::sqrt;
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D adjustPlanResolution(
|
||||
const nav_2d_msgs::msg::Path2D & global_plan_in,
|
||||
double resolution)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D global_plan_out;
|
||||
if (global_plan_in.poses.size() == 0) {
|
||||
return global_plan_out;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Pose2D last = global_plan_in.poses[0];
|
||||
global_plan_out.poses.push_back(last);
|
||||
|
||||
// we can take "holes" in the plan smaller than 2 grid cells (squared = 4)
|
||||
double min_sq_resolution = resolution * resolution * 4.0;
|
||||
|
||||
for (unsigned int i = 1; i < global_plan_in.poses.size(); ++i) {
|
||||
geometry_msgs::msg::Pose2D loop = global_plan_in.poses[i];
|
||||
double sq_dist = (loop.x - last.x) * (loop.x - last.x) + (loop.y - last.y) * (loop.y - last.y);
|
||||
if (sq_dist > min_sq_resolution) {
|
||||
// add points in-between
|
||||
double diff = sqrt(sq_dist) - sqrt(min_sq_resolution);
|
||||
int steps = static_cast<int>(diff / resolution) - 1;
|
||||
double steps_double = static_cast<double>(steps);
|
||||
|
||||
double delta_x = (loop.x - last.x) / steps_double;
|
||||
double delta_y = (loop.y - last.y) / steps_double;
|
||||
double delta_t = (loop.theta - last.theta) / steps_double;
|
||||
|
||||
for (int j = 1; j < steps; ++j) {
|
||||
geometry_msgs::msg::Pose2D pose;
|
||||
pose.x = last.x + j * delta_x;
|
||||
pose.y = last.y + j * delta_y;
|
||||
pose.theta = last.theta + j * delta_t;
|
||||
global_plan_out.poses.push_back(pose);
|
||||
}
|
||||
}
|
||||
global_plan_out.poses.push_back(global_plan_in.poses[i]);
|
||||
last.x = loop.x;
|
||||
last.y = loop.y;
|
||||
}
|
||||
return global_plan_out;
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include "nav_2d_utils/tf_help.hpp"
|
||||
|
||||
namespace nav_2d_utils
|
||||
{
|
||||
|
||||
bool transformPose(
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::string frame,
|
||||
const geometry_msgs::msg::PoseStamped & in_pose,
|
||||
geometry_msgs::msg::PoseStamped & out_pose,
|
||||
rclcpp::Duration & transform_tolerance
|
||||
)
|
||||
{
|
||||
if (in_pose.header.frame_id == frame) {
|
||||
out_pose = in_pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
try {
|
||||
tf->transform(in_pose, out_pose, frame);
|
||||
return true;
|
||||
} catch (tf2::ExtrapolationException & ex) {
|
||||
auto transform = tf->lookupTransform(
|
||||
frame,
|
||||
in_pose.header.frame_id,
|
||||
tf2::TimePointZero
|
||||
);
|
||||
if (
|
||||
(rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) >
|
||||
transform_tolerance)
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("tf_help"),
|
||||
"Transform data too old when converting from %s to %s",
|
||||
in_pose.header.frame_id.c_str(),
|
||||
frame.c_str()
|
||||
);
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("tf_help"),
|
||||
"Data time: %ds %uns, Transform time: %ds %uns",
|
||||
in_pose.header.stamp.sec,
|
||||
in_pose.header.stamp.nanosec,
|
||||
transform.header.stamp.sec,
|
||||
transform.header.stamp.nanosec
|
||||
);
|
||||
return false;
|
||||
} else {
|
||||
tf2::doTransform(in_pose, out_pose, transform);
|
||||
return true;
|
||||
}
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("tf_help"),
|
||||
"Exception in transformPose: %s",
|
||||
ex.what()
|
||||
);
|
||||
return false;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool transformPose(
|
||||
const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::string frame,
|
||||
const nav_2d_msgs::msg::Pose2DStamped & in_pose,
|
||||
nav_2d_msgs::msg::Pose2DStamped & out_pose,
|
||||
rclcpp::Duration & transform_tolerance
|
||||
)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped in_3d_pose = pose2DToPoseStamped(in_pose);
|
||||
geometry_msgs::msg::PoseStamped out_3d_pose;
|
||||
|
||||
bool ret = transformPose(tf, frame, in_3d_pose, out_3d_pose, transform_tolerance);
|
||||
if (ret) {
|
||||
out_pose = poseStampedToPose2D(out_3d_pose);
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
} // namespace nav_2d_utils
|
||||
@@ -0,0 +1,162 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <vector>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav_2d_utils/conversions.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp/time.hpp"
|
||||
|
||||
using nav_2d_utils::posesToPath;
|
||||
using nav_2d_utils::pathToPath;
|
||||
|
||||
TEST(nav_2d_utils, PosesToPathEmpty)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::PoseStamped> poses;
|
||||
nav_msgs::msg::Path path = posesToPath(poses);
|
||||
|
||||
EXPECT_EQ(path.poses.size(), 0ul);
|
||||
}
|
||||
|
||||
TEST(nav_2d_utils, PosesToPathNonEmpty)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::PoseStamped> poses;
|
||||
geometry_msgs::msg::PoseStamped pose1;
|
||||
rclcpp::Time time1, time2;
|
||||
auto node = rclcpp::Node::make_shared("twod_utils_test_node");
|
||||
time1 = node->now();
|
||||
|
||||
tf2::Quaternion quat1, quat2;
|
||||
quat1.setRPY(0, 0, 0.123);
|
||||
pose1.pose.position.x = 1.0;
|
||||
pose1.pose.position.y = 2.0;
|
||||
pose1.pose.orientation.w = quat1.w();
|
||||
pose1.pose.orientation.x = quat1.x();
|
||||
pose1.pose.orientation.y = quat1.y();
|
||||
pose1.pose.orientation.z = quat1.z();
|
||||
pose1.header.stamp = time1;
|
||||
pose1.header.frame_id = "frame1_id";
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose2;
|
||||
pose2.pose.position.x = 4.0;
|
||||
pose2.pose.position.y = 5.0;
|
||||
quat2.setRPY(0, 0, 0.987);
|
||||
pose2.pose.orientation.w = quat2.w();
|
||||
pose2.pose.orientation.x = quat2.x();
|
||||
pose2.pose.orientation.y = quat2.y();
|
||||
pose2.pose.orientation.z = quat2.z();
|
||||
|
||||
time2 = node->now();
|
||||
pose2.header.stamp = time2;
|
||||
pose2.header.frame_id = "frame2_id";
|
||||
|
||||
poses.push_back(pose1);
|
||||
poses.push_back(pose2);
|
||||
|
||||
nav_msgs::msg::Path path = posesToPath(poses);
|
||||
|
||||
EXPECT_EQ(path.poses.size(), 2ul);
|
||||
EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
|
||||
EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
|
||||
EXPECT_EQ(path.poses[0].header.stamp, time1);
|
||||
EXPECT_EQ(path.poses[0].header.frame_id, "frame1_id");
|
||||
EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
|
||||
EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
|
||||
EXPECT_EQ(path.poses[1].header.frame_id, "frame2_id");
|
||||
|
||||
EXPECT_EQ(path.header.stamp, time1);
|
||||
}
|
||||
|
||||
TEST(nav_2d_utils, PathToPathEmpty)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D path2d;
|
||||
nav_msgs::msg::Path path = pathToPath(path2d);
|
||||
EXPECT_EQ(path.poses.size(), 0ul);
|
||||
}
|
||||
|
||||
TEST(nav_2d_utils, PathToPathNoNEmpty)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D path2d;
|
||||
|
||||
geometry_msgs::msg::Pose2D pose1;
|
||||
pose1.x = 1.0;
|
||||
pose1.y = 2.0;
|
||||
pose1.theta = M_PI / 2.0;
|
||||
|
||||
geometry_msgs::msg::Pose2D pose2;
|
||||
pose2.x = 4.0;
|
||||
pose2.y = 5.0;
|
||||
pose2.theta = M_PI;
|
||||
|
||||
path2d.poses.push_back(pose1);
|
||||
path2d.poses.push_back(pose2);
|
||||
|
||||
nav_msgs::msg::Path path = pathToPath(path2d);
|
||||
EXPECT_EQ(path.poses.size(), 2ul);
|
||||
EXPECT_EQ(path.poses[0].pose.position.x, 1.0);
|
||||
EXPECT_EQ(path.poses[0].pose.position.y, 2.0);
|
||||
|
||||
tf2::Quaternion quat;
|
||||
quat.setRPY(0, 0, M_PI / 2.0);
|
||||
EXPECT_EQ(path.poses[0].pose.orientation.w, quat.w());
|
||||
EXPECT_EQ(path.poses[0].pose.orientation.x, quat.x());
|
||||
EXPECT_EQ(path.poses[0].pose.orientation.y, quat.x());
|
||||
EXPECT_EQ(path.poses[0].pose.orientation.z, quat.z());
|
||||
|
||||
EXPECT_EQ(path.poses[1].pose.position.x, 4.0);
|
||||
EXPECT_EQ(path.poses[1].pose.position.y, 5.0);
|
||||
quat.setRPY(0, 0, M_PI);
|
||||
EXPECT_EQ(path.poses[1].pose.orientation.w, quat.w());
|
||||
EXPECT_EQ(path.poses[1].pose.orientation.x, quat.x());
|
||||
EXPECT_EQ(path.poses[1].pose.orientation.y, quat.x());
|
||||
EXPECT_EQ(path.poses[1].pose.orientation.z, quat.z());
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
::testing::InitGoogleTest(&argc, argv);
|
||||
|
||||
// initialize ROS
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
bool all_successful = RUN_ALL_TESTS();
|
||||
|
||||
// shutdown ROS
|
||||
rclcpp::shutdown();
|
||||
|
||||
return all_successful;
|
||||
}
|
||||
@@ -0,0 +1,9 @@
|
||||
ament_add_gtest(2d_utils_tests 2d_utils_test.cpp)
|
||||
target_link_libraries(2d_utils_tests conversions)
|
||||
|
||||
ament_add_gtest(path_ops_tests path_ops_test.cpp)
|
||||
target_link_libraries(path_ops_tests path_ops)
|
||||
|
||||
ament_add_gtest(tf_help_tests tf_help_test.cpp)
|
||||
target_link_libraries(tf_help_tests tf_help conversions)
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include <cmath>
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav_2d_utils/path_ops.hpp"
|
||||
|
||||
using std::sqrt;
|
||||
using nav_2d_utils::adjustPlanResolution;
|
||||
|
||||
TEST(path_ops_test, AdjustResolutionEmpty)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D in;
|
||||
nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, 2.0);
|
||||
EXPECT_EQ(out.poses.size(), 0ul);
|
||||
}
|
||||
|
||||
TEST(path_ops_test, AdjustResolutionSimple)
|
||||
{
|
||||
nav_2d_msgs::msg::Path2D in;
|
||||
const float RESOLUTION = 20.0;
|
||||
|
||||
geometry_msgs::msg::Pose2D pose1;
|
||||
pose1.x = 0.0;
|
||||
pose1.y = 0.0;
|
||||
geometry_msgs::msg::Pose2D pose2;
|
||||
pose2.x = 100.0;
|
||||
pose2.y = 0.0;
|
||||
|
||||
in.poses.push_back(pose1);
|
||||
in.poses.push_back(pose2);
|
||||
|
||||
nav_2d_msgs::msg::Path2D out = adjustPlanResolution(in, RESOLUTION);
|
||||
float length = 100;
|
||||
uint32_t number_of_points = ceil(length / (2 * RESOLUTION));
|
||||
EXPECT_EQ(out.poses.size(), number_of_points);
|
||||
float max_length = length / (number_of_points - 1);
|
||||
|
||||
for (unsigned int i = 1; i < out.poses.size(); i++) {
|
||||
pose1 = out.poses[i - 1];
|
||||
pose2 = out.poses[i];
|
||||
|
||||
double sq_dist = (pose1.x - pose2.x) * (pose1.x - pose2.x) +
|
||||
(pose1.y - pose2.y) * (pose1.y - pose2.y);
|
||||
|
||||
EXPECT_TRUE(sqrt(sq_dist) <= max_length);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Wilco Bonestroo
|
||||
* 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 the copyright holder 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 HOLDER 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.
|
||||
*/
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "nav_2d_utils/tf_help.hpp"
|
||||
|
||||
TEST(TF_Help, TransformToSelf) {
|
||||
bool result;
|
||||
|
||||
std::shared_ptr<tf2_ros::Buffer> tf;
|
||||
std::string frame = "frame_id";
|
||||
geometry_msgs::msg::PoseStamped in_pose;
|
||||
in_pose.header.frame_id = "frame_id";
|
||||
in_pose.pose.position.x = 1.0;
|
||||
in_pose.pose.position.y = 2.0;
|
||||
in_pose.pose.position.z = 3.0;
|
||||
tf2::Quaternion qt;
|
||||
qt.setRPY(0.5, 1.0, 1.5);
|
||||
in_pose.pose.orientation.w = qt.w();
|
||||
in_pose.pose.orientation.x = qt.x();
|
||||
in_pose.pose.orientation.y = qt.y();
|
||||
in_pose.pose.orientation.z = qt.z();
|
||||
|
||||
geometry_msgs::msg::PoseStamped out_pose;
|
||||
rclcpp::Duration transform_tolerance(0, 500);
|
||||
|
||||
result = nav_2d_utils::transformPose(tf, frame, in_pose, out_pose, transform_tolerance);
|
||||
|
||||
EXPECT_TRUE(result);
|
||||
EXPECT_EQ(out_pose.header.frame_id, "frame_id");
|
||||
EXPECT_EQ(out_pose.pose.position.x, 1.0);
|
||||
EXPECT_EQ(out_pose.pose.position.y, 2.0);
|
||||
EXPECT_EQ(out_pose.pose.position.z, 3.0);
|
||||
EXPECT_EQ(out_pose.pose.orientation.w, qt.w());
|
||||
EXPECT_EQ(out_pose.pose.orientation.x, qt.x());
|
||||
EXPECT_EQ(out_pose.pose.orientation.y, qt.y());
|
||||
EXPECT_EQ(out_pose.pose.orientation.z, qt.z());
|
||||
}
|
||||
|
||||
TEST(TF_Help, EmptyBuffer) {
|
||||
auto clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
|
||||
auto buffer = std::make_shared<tf2_ros::Buffer>(clock);
|
||||
|
||||
std::string frame = "frame_id";
|
||||
geometry_msgs::msg::PoseStamped in_pose;
|
||||
in_pose.header.frame_id = "other_frame_id";
|
||||
in_pose.pose.position.x = 1.0;
|
||||
in_pose.pose.position.y = 2.0;
|
||||
in_pose.pose.position.z = 3.0;
|
||||
tf2::Quaternion qt;
|
||||
qt.setRPY(0.5, 1.0, 1.5);
|
||||
in_pose.pose.orientation.w = qt.w();
|
||||
in_pose.pose.orientation.x = qt.x();
|
||||
in_pose.pose.orientation.y = qt.y();
|
||||
in_pose.pose.orientation.z = qt.z();
|
||||
|
||||
geometry_msgs::msg::PoseStamped out_pose;
|
||||
rclcpp::Duration transform_tolerance(0, 500);
|
||||
|
||||
bool result;
|
||||
result = nav_2d_utils::transformPose(buffer, frame, in_pose, out_pose, transform_tolerance);
|
||||
|
||||
EXPECT_FALSE(result);
|
||||
}
|
||||
Reference in New Issue
Block a user