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

142
navigations/move_base_flex/mbf_simple_nav/src/simple_navigation_server.cpp Executable file
View File

@@ -0,0 +1,142 @@
/*
* 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.
*
* simple_navigation_server.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "mbf_simple_nav/simple_navigation_server.h"
namespace mbf_simple_nav
{
SimpleNavigationServer::SimpleNavigationServer(const TFPtr &tf_listener_ptr) :
mbf_abstract_nav::AbstractNavigationServer(tf_listener_ptr),
planner_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractPlanner"),
controller_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractController"),
recovery_plugin_loader_("mbf_abstract_core", "mbf_abstract_core::AbstractRecovery")
{
// initialize all plugins
initializeServerComponents();
// start all action servers
startActionServers();
}
mbf_abstract_core::AbstractPlanner::Ptr SimpleNavigationServer::loadPlannerPlugin(const std::string& planner_type)
{
mbf_abstract_core::AbstractPlanner::Ptr planner_ptr;
ROS_INFO("Load global planner plugin.");
try
{
planner_ptr = planner_plugin_loader_.createInstance(planner_type);
}
catch (const pluginlib::PluginlibException &ex)
{
ROS_FATAL_STREAM("Failed to load the " << planner_type << " planner, are you sure it is properly registered"
<< " and that the containing library is built? Exception: " << ex.what());
}
ROS_INFO("Global planner plugin loaded.");
return planner_ptr;
}
bool SimpleNavigationServer::initializePlannerPlugin(
const std::string& name,
const mbf_abstract_core::AbstractPlanner::Ptr& planner_ptr
)
{
return true;
}
mbf_abstract_core::AbstractController::Ptr SimpleNavigationServer::loadControllerPlugin(
const std::string& controller_type)
{
mbf_abstract_core::AbstractController::Ptr controller_ptr;
ROS_DEBUG("Load controller plugin.");
try
{
controller_ptr = controller_plugin_loader_.createInstance(controller_type);
ROS_INFO_STREAM("MBF_core-based local planner plugin " << controller_type << " loaded");
}
catch (const pluginlib::PluginlibException &ex)
{
ROS_FATAL_STREAM("Failed to load the " << controller_type
<< " local planner, are you sure it's properly registered"
<< " and that the containing library is built? Exception: " << ex.what());
}
return controller_ptr;
}
bool SimpleNavigationServer::initializeControllerPlugin(
const std::string& name,
const mbf_abstract_core::AbstractController::Ptr& controller_ptr)
{
return true;
}
mbf_abstract_core::AbstractRecovery::Ptr SimpleNavigationServer::loadRecoveryPlugin(
const std::string& recovery_type)
{
mbf_abstract_core::AbstractRecovery::Ptr recovery_ptr;
try
{
recovery_ptr = boost::static_pointer_cast<mbf_abstract_core::AbstractRecovery>(
recovery_plugin_loader_.createInstance(recovery_type));
}
catch (pluginlib::PluginlibException &ex)
{
ROS_FATAL_STREAM("Failed to load the " << recovery_type << " recovery behavior, are you sure it's properly registered"
<< " and that the containing library is built? Exception: " << ex.what());
}
return recovery_ptr;
}
bool SimpleNavigationServer::initializeRecoveryPlugin(
const std::string& name,
const mbf_abstract_core::AbstractRecovery::Ptr& behavior_ptr)
{
return true;
}
SimpleNavigationServer::~SimpleNavigationServer()
{
}
} /* namespace mbf_simple_nav */

69
navigations/move_base_flex/mbf_simple_nav/src/simple_server_node.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* 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.
*
* simple_server_node.cpp
*
* authors:
* Sebastian Pütz <spuetz@uni-osnabrueck.de>
* Jorge Santos Simón <santos@magazino.eu>
*
*/
#include "mbf_simple_nav/simple_navigation_server.h"
#include <mbf_utility/types.h>
#include <tf2_ros/transform_listener.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "mbf_simple_server");
typedef boost::shared_ptr<mbf_simple_nav::SimpleNavigationServer> SimpleNavigationServerPtr;
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
double cache_time;
private_nh.param("tf_cache_time", cache_time, 10.0);
#ifdef USE_OLD_TF
TFPtr tf_listener_ptr(new TF(nh, ros::Duration(cache_time), true));
#else
TFPtr tf_listener_ptr(new TF(ros::Duration(cache_time)));
tf2_ros::TransformListener tf_listener(*tf_listener_ptr);
#endif
SimpleNavigationServerPtr controller_ptr(
new mbf_simple_nav::SimpleNavigationServer(tf_listener_ptr));
ros::spin();
return EXIT_SUCCESS;
}