HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
navigations/move_base_flex/mbf_utility/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,42 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mbf_utility
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.2 (2020-05-25)
------------------
* Remove dependency on base_local_planner and move FootprintHelper class to mbf_costmap_nav and make it static
0.3.1 (2020-04-07)
------------------
0.3.0 (2020-03-31)
------------------
* Add exception classes for get_path, exe_path and recovery
* unify license declaration to BSD-3
0.2.5 (2019-10-11)
------------------
0.2.4 (2019-06-16)
------------------
* Add check_point_cost service
0.2.3 (2018-11-14)
------------------
* Fix getRobotPose in melodic
0.2.2 (2018-10-10)
------------------
0.2.1 (2018-10-03)
------------------
* Make MBF melodic and indigo compatible
* Fix GoalHandle references bug in callbacks
0.2.0 (2018-09-11)
------------------
* Update copyright and 3-clause-BSD license
0.1.0 (2018-03-22)
------------------
* First release of move_base_flex for kinetic and lunar

42
navigations/move_base_flex/mbf_utility/CMakeLists.txt Executable file
View File

@@ -0,0 +1,42 @@
cmake_minimum_required(VERSION 2.8.3)
project(mbf_utility)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
roscpp
tf
tf2
tf2_ros
tf2_geometry_msgs
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES mbf_utility
CATKIN_DEPENDS geometry_msgs roscpp tf tf2 tf2_ros tf2_geometry_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/navigation_utility.cpp
src/robot_information.cpp
)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

43
navigations/move_base_flex/mbf_utility/include/mbf_utility/exe_path_exception.h Executable file
View File

@@ -0,0 +1,43 @@
#ifndef MBF_UTILITY__EXE_PATH_EXCEPTION_H_
#define MBF_UTILITY__EXE_PATH_EXCEPTION_H_
#include <exception>
#include <mbf_msgs/ExePathResult.h>
namespace mbf_utility
{
struct ExePathException : public std::exception
{
ExePathException(unsigned int error_code) : outcome(error_code){}
const char * what () const throw () {
switch(outcome)
{
case mbf_msgs::ExePathResult::FAILURE: return "Failure";
case mbf_msgs::ExePathResult::CANCELED: return "Canceled";
case mbf_msgs::ExePathResult::NO_VALID_CMD: return "No valid command";
case mbf_msgs::ExePathResult::PAT_EXCEEDED: return "Patience exceeded";
case mbf_msgs::ExePathResult::COLLISION: return "Collision";
case mbf_msgs::ExePathResult::OSCILLATION: return "Oscillation";
case mbf_msgs::ExePathResult::ROBOT_STUCK: return "Robot stuck";
case mbf_msgs::ExePathResult::MISSED_GOAL: return "Missed Goal";
case mbf_msgs::ExePathResult::MISSED_PATH: return "Missed Path";
case mbf_msgs::ExePathResult::BLOCKED_PATH: return "Blocked Path";
case mbf_msgs::ExePathResult::INVALID_PATH: return "Invalid Path";
case mbf_msgs::ExePathResult::TF_ERROR: return "TF Error";
case mbf_msgs::ExePathResult::NOT_INITIALIZED: return "Not initialized";
case mbf_msgs::ExePathResult::INVALID_PLUGIN: return "Invalid Plugin";
case mbf_msgs::ExePathResult::INTERNAL_ERROR: return "Internal Error";
case mbf_msgs::ExePathResult::STOPPED: return "Stopped";
case mbf_msgs::ExePathResult::OUT_OF_MAP: return "Out of map";
case mbf_msgs::ExePathResult::MAP_ERROR: return "Map error";
default: return "unknown error code: " + outcome;
}
}
unsigned int outcome;
};
} /* namespace mbf_utility */
#endif // MBF_UTILITY__EXE_PATH_EXCEPTION_H_

39
navigations/move_base_flex/mbf_utility/include/mbf_utility/get_path_exception.h Executable file
View File

@@ -0,0 +1,39 @@
#ifndef MBF_UTILITY__GET_PATH_EXCEPTION_H_
#define MBF_UTILITY__GET_PATH_EXCEPTION_H_
#include <exception>
#include <mbf_msgs/GetPathResult.h>
namespace mbf_utility
{
struct GetPathException : public std::exception
{
GetPathException(unsigned int error_code) : outcome(error_code){}
const char * what () const throw () {
switch(outcome)
{
case mbf_msgs::GetPathResult::FAILURE: return "Failure";
case mbf_msgs::GetPathResult::CANCELED: return "Canceled";
case mbf_msgs::GetPathResult::INVALID_START: return "Invalid start"
case mbf_msgs::GetPathResult::INVALID_GOAL: return "Invalid goal"
case mbf_msgs::GetPathResult::NO_PATH_FOUND: return "No path found";
case mbf_msgs::GetPathResult::PAT_EXCEEDED: return "Patience exceeded";
case mbf_msgs::GetPathResult::EMPTY_PATH return "Empty Path";
case mbf_msgs::GetPathResult::TF_ERROR: return "TF Error";
case mbf_msgs::GetPathResult::NOT_INITIALIZED: return "Not initialized";
case mbf_msgs::GetPathResult::INVALID_PLUGIN: return "Invalid Plugin";
case mbf_msgs::GetPathResult::INTERNAL_ERROR: return "Internal Error";
case mbf_msgs::GetPathResult::STOPPED: return "Stopped";
case mbf_msgs::GetPathResult::OUT_OF_MAP: return "Out of map";
case mbf_msgs::GetPathResult::MAP_ERROR: return "Map error";
default: return "unknown error code: " + outcome;
}
}
unsigned int outcome;
};
} /* namespace mbf_utility */
#endif // MBF_UTILITY__GET_PATH_EXCEPTION_H_

119
navigations/move_base_flex/mbf_utility/include/mbf_utility/navigation_utility.h Executable file
View File

@@ -0,0 +1,119 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. 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.
*
* navigation_utility.h
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#ifndef MBF_UTILITY__NAVIGATION_UTILITY_H_
#define MBF_UTILITY__NAVIGATION_UTILITY_H_
#include <geometry_msgs/PoseStamped.h>
#include <ros/duration.h>
#include <ros/time.h>
#include <string>
#include <tf2/convert.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include "mbf_utility/types.h"
namespace mbf_utility
{
/**
* @brief Transforms a point from one frame into another.
* @param tf_listener TransformListener.
* @param target_frame Target frame for the point.
* @param timeout Timeout for looking up the transformation.
* @param in Point to transform.
* @param out Transformed point.
* @return true, if the transformation succeeded.
*/
bool transformPoint(const TF &tf,
const std::string &target_frame,
const ros::Duration &timeout,
const geometry_msgs::PointStamped &in,
geometry_msgs::PointStamped &out);
/**
* @brief Transforms a pose from one frame into another.
* @param tf_listener TransformListener.
* @param target_frame Target frame for the pose.
* @param timeout Timeout for looking up the transformation.
* @param in Pose to transform.
* @param out Transformed pose.
* @return true, if the transformation succeeded.
*/
bool transformPose(const TF &tf,
const std::string &target_frame,
const ros::Duration &timeout,
const geometry_msgs::PoseStamped &in,
geometry_msgs::PoseStamped &out);
/**
* @brief Computes the robot pose.
* @param tf_listener TransformListener.
* @param robot_frame frame of the robot.
* @param global_frame global frame in which the robot is located.
* @param timeout Timeout for looking up the transformation.
* @param robot_pose the computed rebot pose in the global frame.
* @return true, if succeeded, false otherwise.
*/
bool getRobotPose(const TF &tf,
const std::string &robot_frame,
const std::string &global_frame,
const ros::Duration &timeout,
geometry_msgs::PoseStamped &robot_pose);
/**
* @brief Computes the Euclidean-distance between two poses.
* @param pose1 pose 1
* @param pose2 pose 2
* @return Euclidean distance between pose 1 and pose 2.
*/
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2);
/**
* @brief computes the smallest angle between two poses.
* @param pose1 pose 1
* @param pose2 pose 2
* @return smallest angle between pose 1 and pose 2.
*/
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2);
} /* namespace mbf_utility */
#endif /* navigation_utility.h */

33
navigations/move_base_flex/mbf_utility/include/mbf_utility/recovery_exception.h Executable file
View File

@@ -0,0 +1,33 @@
#ifndef MBF_UTILITY__RECOVERY_EXCEPTION_H_
#define MBF_UTILITY__RECOVERY_EXCEPTION_H_
#include <exception>
#include <mbf_msgs/RecoveryResult.h>
namespace mbf_utility
{
struct RecoveryException : public std::exception
{
RecoveryException(unsigned int error_code) : outcome(error_code){}
const char * what () const throw () {
switch(outcome)
{
case mbf_msgs::RecoveryResult::FAILURE: return "Failure";
case mbf_msgs::RecoveryResult::CANCELED: return "Canceled";
case mbf_msgs::RecoveryResult::PAT_EXCEEDED: return "Patience exceeded";
case mbf_msgs::RecoveryResult::TF_ERROR: return "TF Error";
case mbf_msgs::RecoveryResult::NOT_INITIALIZED: return "Not initialized";
case mbf_msgs::RecoveryResult::INVALID_PLUGIN: return "Invalid Plugin";
case mbf_msgs::RecoveryResult::INTERNAL_ERROR: return "Internal Error";
case mbf_msgs::RecoveryResult::STOPPED: return "Stopped";
default: return "unknown error code: " + outcome;
}
}
unsigned int outcome;
};
} /* namespace mbf_utility */
#endif // MBF_UTILITY__RECOVERY_EXCEPTION_H_

95
navigations/move_base_flex/mbf_utility/include/mbf_utility/robot_information.h Executable file
View File

@@ -0,0 +1,95 @@
/*
* Copyright 2018, Sebastian Pütz
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. 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.
*
* robot_information.h
*
* author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
*
*/
#ifndef MBF_UTILITY__ROBOT_INFORMATION_H_
#define MBF_UTILITY__ROBOT_INFORMATION_H_
#include <boost/shared_ptr.hpp>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/duration.h>
#include <string>
#include "mbf_utility/types.h"
namespace mbf_utility
{
class RobotInformation
{
public:
typedef boost::shared_ptr<RobotInformation> Ptr;
RobotInformation(
TF &tf_listener,
const std::string &global_frame,
const std::string &robot_frame,
const ros::Duration &tf_timeout);
/**
* @brief Computes the current robot pose (robot_frame_) in the global frame (global_frame_).
* @param robot_pose Reference to the robot_pose message object to be filled.
* @return true, if the current robot pose could be computed, false otherwise.
*/
bool getRobotPose(geometry_msgs::PoseStamped &robot_pose) const;
bool getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const;
const std::string& getGlobalFrame() const;
const std::string& getRobotFrame() const;
const TF& getTransformListener() const;
const ros::Duration& getTfTimeout() const;
private:
const TF& tf_listener_;
const std::string &global_frame_;
const std::string &robot_frame_;
const ros::Duration &tf_timeout_;
};
} /* mbf_utility */
#endif /* MBF_UTILITY__ROBOT_INFORMATION_H_ */

60
navigations/move_base_flex/mbf_utility/include/mbf_utility/types.h Executable file
View File

@@ -0,0 +1,60 @@
/*
* Copyright 2018, Sebastian Pütz
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. 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.
*
* abstract_planner.h
*
* author: Jannik Abbenseth <jba@ipa.fhg.de>
*
*/
#ifndef MBF_UTILITY__TYPES_H_
#define MBF_UTILITY__TYPES_H_
#include <boost/shared_ptr.hpp>
#include <ros/common.h>
#if ROS_VERSION_MINIMUM (1, 14, 0) // if current ros version is >= 1.14.0
// Melodic uses TF2
#include <tf2_ros/buffer.h>
typedef boost::shared_ptr<tf2_ros::Buffer> TFPtr;
typedef tf2_ros::Buffer TF;
typedef tf2::TransformException TFException;
#else
// Previous versions still using TF
#define USE_OLD_TF
#include <tf/transform_listener.h>
typedef boost::shared_ptr<tf::TransformListener> TFPtr;
typedef tf::TransformListener TF;
typedef tf::TransformException TFException;
#endif
#endif

21
navigations/move_base_flex/mbf_utility/package.xml Executable file
View File

@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<package format="2">
<name>mbf_utility</name>
<version>0.3.2</version>
<description>The mbf_utility package</description>
<maintainer email="spuetz@uos.de">Sebastian Pütz</maintainer>
<license>BSD-3</license>
<url type="website">http://wiki.ros.org/move_base_flex/mbf_utility</url>
<author email="spuetz@uos.de">Sebastian Pütz</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>roscpp</depend>
<depend>tf</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
</package>

186
navigations/move_base_flex/mbf_utility/src/navigation_utility.cpp Executable file
View File

@@ -0,0 +1,186 @@
/*
* Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. 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.
*
* navigation_utility.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include <tf/tf.h>
#include "mbf_utility/navigation_utility.h"
namespace mbf_utility
{
bool getRobotPose(const TF &tf,
const std::string &robot_frame,
const std::string &global_frame,
const ros::Duration &timeout,
geometry_msgs::PoseStamped &robot_pose)
{
geometry_msgs::PoseStamped local_pose;
local_pose.header.frame_id = robot_frame;
local_pose.header.stamp = ros::Time(0); // most recent available
local_pose.pose.orientation.w = 1.0;
bool success = transformPose(tf,
global_frame,
timeout,
local_pose,
robot_pose);
if (success && ros::Time::now() - robot_pose.header.stamp > timeout)
{
ROS_WARN("Most recent robot pose is %gs old (tolerance %gs)",
(ros::Time::now() - robot_pose.header.stamp).toSec(), timeout.toSec());
return false;
}
return success;
}
bool transformPose(const TF &tf,
const std::string &target_frame,
const ros::Duration &timeout,
const geometry_msgs::PoseStamped &in,
geometry_msgs::PoseStamped &out)
{
std::string error_msg;
#ifdef USE_OLD_TF
bool success = tf.waitForTransform(target_frame,
in.header.frame_id,
in.header.stamp,
timeout,
ros::Duration(0.01),
&error_msg);
#else
bool success = tf.canTransform(target_frame,
in.header.frame_id,
in.header.stamp,
timeout,
&error_msg);
#endif
if (!success)
{
ROS_WARN_STREAM("Failed to look up transform from frame '" << in.header.frame_id << "' into frame '" << target_frame
<< "': " << error_msg);
return false;
}
try
{
#ifdef USE_OLD_TF
tf.transformPose(target_frame, in, out);
#else
tf.transform(in, out, target_frame);
#endif
}
catch (const TFException &ex)
{
ROS_WARN_STREAM("Failed to transform pose from frame '" << in.header.frame_id << " ' into frame '"
<< target_frame << "' with exception: " << ex.what());
return false;
}
return true;
}
bool transformPoint(const TF &tf,
const std::string &target_frame,
const ros::Duration &timeout,
const geometry_msgs::PointStamped &in,
geometry_msgs::PointStamped &out)
{
std::string error_msg;
#ifdef USE_OLD_TF
bool success = tf.waitForTransform(target_frame,
in.header.frame_id,
in.header.stamp,
timeout,
ros::Duration(0.01),
&error_msg);
#else
bool success = tf.canTransform(target_frame,
in.header.frame_id,
in.header.stamp,
timeout,
&error_msg);
#endif
if (!success)
{
ROS_WARN_STREAM("Failed to look up transform from frame '" << in.header.frame_id << "' into frame '" << target_frame
<< "': " << error_msg);
return false;
}
try
{
#ifdef USE_OLD_TF
tf.transformPoint(target_frame, in, out);
#else
tf.transform(in, out, target_frame);
#endif
}
catch (const TFException &ex)
{
ROS_WARN_STREAM("Failed to transform point from frame '" << in.header.frame_id << " ' into frame '"
<< target_frame << "' with exception: " << ex.what());
return false;
}
return true;
}
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
{
const geometry_msgs::Point &p1 = pose1.pose.position;
const geometry_msgs::Point &p2 = pose2.pose.position;
const double dx = p1.x - p2.x;
const double dy = p1.y - p2.y;
const double dz = p1.z - p2.z;
return sqrt(dx * dx + dy * dy + dz * dz);
}
double angle(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
{
const geometry_msgs::Quaternion &q1 = pose1.pose.orientation;
const geometry_msgs::Quaternion &q2 = pose2.pose.orientation;
tf::Quaternion rot1, rot2;
tf::quaternionMsgToTF(q1, rot1);
tf::quaternionMsgToTF(q2, rot2);
return rot1.angleShortestPath(rot2);
}
} /* namespace mbf_utility */

82
navigations/move_base_flex/mbf_utility/src/robot_information.cpp Executable file
View File

@@ -0,0 +1,82 @@
/*
* Copyright 2018, Sebastian Pütz
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* 2. 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.
*
* 3. 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.
*
* robot_information.cpp
*
* author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
*
*/
#include "mbf_utility/robot_information.h"
#include "mbf_utility/navigation_utility.h"
namespace mbf_utility
{
RobotInformation::RobotInformation(TF &tf_listener,
const std::string &global_frame,
const std::string &robot_frame,
const ros::Duration &tf_timeout)
: tf_listener_(tf_listener), global_frame_(global_frame), robot_frame_(robot_frame), tf_timeout_(tf_timeout)
{
}
bool RobotInformation::getRobotPose(geometry_msgs::PoseStamped &robot_pose) const
{
bool tf_success = mbf_utility::getRobotPose(tf_listener_, robot_frame_, global_frame_,
ros::Duration(tf_timeout_), robot_pose);
if (!tf_success)
{
ROS_ERROR_STREAM("Can not get the robot pose in the global frame. - robot frame: \""
<< robot_frame_ << "\" global frame: \"" << global_frame_ << std::endl);
return false;
}
return true;
}
bool RobotInformation::getRobotVelocity(geometry_msgs::TwistStamped &robot_velocity, ros::Duration look_back_duration) const
{
// TODO implement and filter tf data to compute velocity.
return false;
}
const std::string& RobotInformation::getGlobalFrame() const {return global_frame_;};
const std::string& RobotInformation::getRobotFrame() const {return robot_frame_;};
const TF& RobotInformation::getTransformListener() const {return tf_listener_;};
const ros::Duration& RobotInformation::getTfTimeout() const {return tf_timeout_;}
} /* namespace mbf_utility */