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

update ros

Browse Source
This commit is contained in:
HiepLM 2026-01-07 09:28:55 +07:00
parent 7cea6bb120
commit ec6d5746e3
58 changed files with 22752 additions and 3773 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
Controllers/Packages/amr_control/CMakeLists.txt
View File

@ -18,6 +18,8 @@ find_package(catkin REQUIRED COMPONENTS
loc_core loc_core
amr_comunication amr_comunication
vda5050_msgs vda5050_msgs
robot_nav_2d_utils
move_base_core move_base_core
robot_cpp robot_cpp
) )
@ -26,83 +28,6 @@ find_package(catkin REQUIRED COMPONENTS
find_package(Boost REQUIRED COMPONENTS system thread) find_package(Boost REQUIRED COMPONENTS system thread)
find_package(PkgConfig) find_package(PkgConfig)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
################################### ###################################
@ -115,7 +40,14 @@ find_package(PkgConfig)
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES amr_control LIBRARIES amr_control
CATKIN_DEPENDS geometry_msgs loc_core nav_2d_utils roscpp std_msgs move_base_core robot_cpp CATKIN_DEPENDS
geometry_msgs
loc_core
nav_2d_utils
move_base_core
robot_cpp
roscpp
std_msgs
DEPENDS Boost DEPENDS Boost
) )
@ -137,14 +69,15 @@ add_library(${PROJECT_NAME}
src/amr_monitor.cpp src/amr_monitor.cpp
src/amr_safety.cpp src/amr_safety.cpp
src/amr_opc_ua_server_api.cpp src/amr_opc_ua_server_api.cpp
# src/amr_vda_5050_client_api.cpp src/amr_vda_5050_client_api.cpp
# src/amr_make_plan_with_order.cpp src/amr_make_plan_with_order.cpp
) )
## Add cmake target dependencies of the library ## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries ## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure ## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings # Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
# Libraries will be found via LD_LIBRARY_PATH or system paths # Libraries will be found via LD_LIBRARY_PATH or system paths
set_target_properties(${PROJECT_NAME} PROPERTIES set_target_properties(${PROJECT_NAME} PROPERTIES
@ -156,7 +89,7 @@ set_target_properties(${PROJECT_NAME} PROPERTIES
## Declare a C++ executable ## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context ## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide ## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp) add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp)
# add_executable(vda_5050_api_test test/vda_5050_api.cpp) # add_executable(vda_5050_api_test test/vda_5050_api.cpp)
## Rename C++ executable without prefix ## Rename C++ executable without prefix
@ -167,18 +100,19 @@ set_target_properties(${PROJECT_NAME} PROPERTIES
## Add cmake target dependencies of the executable ## Add cmake target dependencies of the executable
## same as for the library above ## same as for the library above
# add_dependencies(${PROJECT_NAME}_node
# ${PROJECT_NAME} add_dependencies(${PROJECT_NAME}_node
# ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}
# ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}
# ) ${catkin_EXPORTED_TARGETS}
)
# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings # Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
# Libraries will be found via LD_LIBRARY_PATH or system paths # Libraries will be found via LD_LIBRARY_PATH or system paths
# set_target_properties(${PROJECT_NAME}_node PROPERTIES set_target_properties(${PROJECT_NAME}_node PROPERTIES
# SKIP_BUILD_RPATH TRUE SKIP_BUILD_RPATH TRUE
# BUILD_WITH_INSTALL_RPATH FALSE BUILD_WITH_INSTALL_RPATH FALSE
# INSTALL_RPATH_USE_LINK_PATH FALSE INSTALL_RPATH_USE_LINK_PATH FALSE
# ) )
# add_dependencies(vda_5050_api_test # add_dependencies(vda_5050_api_test
# ${PROJECT_NAME} # ${PROJECT_NAME}
@ -200,11 +134,11 @@ target_link_libraries(${PROJECT_NAME}
${Boost_LIBRARIES} ${Boost_LIBRARIES}
) )
# target_link_libraries(${PROJECT_NAME}_node target_link_libraries(${PROJECT_NAME}_node
# ${PROJECT_NAME} ${PROJECT_NAME}
# ${catkin_LIBRARIES} ${catkin_LIBRARIES}
# ${Boost_LIBRARIES} ${Boost_LIBRARIES}
# ) )
# target_link_libraries(vda_5050_api_test # target_link_libraries(vda_5050_api_test
# ${PROJECT_NAME} # ${PROJECT_NAME}
@ -229,9 +163,9 @@ target_link_libraries(${PROJECT_NAME}
## Mark executables for installation ## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# ) )
## Mark libraries for installation ## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html

17
Controllers/Packages/amr_control/package.xml
View File

@ -63,9 +63,6 @@
<build_depend>loc_core</build_depend> <build_depend>loc_core</build_depend>
<build_depend>amr_comunication</build_depend> <build_depend>amr_comunication</build_depend>
<build_depend>vda5050_msgs</build_depend> <build_depend>vda5050_msgs</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>move_base_core</build_depend>
<build_export_depend>geometry_msgs</build_export_depend> <build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>nav_2d_utils</build_export_depend> <build_export_depend>nav_2d_utils</build_export_depend>
<build_export_depend>std_msgs</build_export_depend> <build_export_depend>std_msgs</build_export_depend>
@ -75,9 +72,6 @@
<build_export_depend>loc_core</build_export_depend> <build_export_depend>loc_core</build_export_depend>
<build_export_depend>amr_comunication</build_export_depend> <build_export_depend>amr_comunication</build_export_depend>
<build_export_depend>vda5050_msgs</build_export_depend> <build_export_depend>vda5050_msgs</build_export_depend>
<build_export_depend>robot_cpp</build_export_depend>
<build_export_depend>move_base_core</build_export_depend>
<exec_depend>geometry_msgs</exec_depend> <exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_2d_utils</exec_depend> <exec_depend>nav_2d_utils</exec_depend>
<exec_depend>std_msgs</exec_depend> <exec_depend>std_msgs</exec_depend>
@ -87,7 +81,18 @@
<exec_depend>loc_core</exec_depend> <exec_depend>loc_core</exec_depend>
<exec_depend>amr_comunication</exec_depend> <exec_depend>amr_comunication</exec_depend>
<exec_depend>vda5050_msgs</exec_depend> <exec_depend>vda5050_msgs</exec_depend>
<!-- PNKX core -->
<build_depend>robot_cpp</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>move_base_core</build_depend>
<build_export_depend>robot_cpp</build_export_depend>
<build_export_depend>robot_nav_2d_utils</build_export_depend>
<build_export_depend>move_base_core</build_export_depend>
<exec_depend>robot_cpp</exec_depend> <exec_depend>robot_cpp</exec_depend>
<exec_depend>robot_nav_2d_utils</exec_depend>
<exec_depend>move_base_core</exec_depend> <exec_depend>move_base_core</exec_depend>
<!-- The export tag contains other, unspecified, tags --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>

674
Controllers/Packages/amr_control/src/amr_control.cpp
View File

@ -233,23 +233,23 @@ namespace amr_control
move_base_ptr_ = move_base_loader_(); move_base_ptr_ = move_base_loader_();
// move_base_ptr_->initialize(tf_core_ptr_); // move_base_ptr_->initialize(tf_core_ptr_);
// ros::Rate r(3); ros::Rate r(3);
// do do
// { {
// r.sleep(); r.sleep();
// ros::spinOnce(); ros::spinOnce();
// } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready); } while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);
// if (move_base_ptr_ != nullptr && if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr && move_base_ptr_->getFeedback() != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready) move_base_ptr_->getFeedback()->is_ready)
// { {
// geometry_msgs::Vector3 linear; robot_geometry_msgs::Vector3 linear;
// linear.x = 0.3; linear.x = 0.3;
// move_base_ptr_->setTwistLinear(linear); move_base_ptr_->setTwistLinear(linear);
// linear.x = -0.3; linear.x = -0.3;
// move_base_ptr_->setTwistLinear(linear); move_base_ptr_->setTwistLinear(linear);
// } }
} }
catch (const std::exception &e) catch (const std::exception &e)
{ {
@ -263,325 +263,325 @@ namespace amr_control
} }
} }
// void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh) void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
// { {
// this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh); this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
// } }
// void AmrController::ArmCallBack() void AmrController::ArmCallBack()
// { {
// ROS_INFO("Arm Calling"); ROS_INFO("Arm Calling");
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// { {
// arm_joined_ = true; arm_joined_ = true;
// this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this); this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
// } }
// } }
// void AmrController::ArmDotuff() void AmrController::ArmDotuff()
// { {
// std::shared_ptr<imr_nova_control> arm_control_ptr; std::shared_ptr<imr_nova_control> arm_control_ptr;
// arm_control_ptr = std::make_shared<imr_nova_control>(); arm_control_ptr = std::make_shared<imr_nova_control>();
// arm_control_ptr->enable_ = &this->enable_; arm_control_ptr->enable_ = &this->enable_;
// arm_control_ptr->go_home_flag_ = &this->arm_go_home_; arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
// arm_control_ptr->continue_flag_ = &this->arm_continue_; arm_control_ptr->continue_flag_ = &this->arm_continue_;
// arm_control_ptr->power_on_flag_ = &this->arm_power_on_; arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
// OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_; OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
// this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_); this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);
// if (!this->arm_go_home_) if (!this->arm_go_home_)
// { {
// arm_control_ptr->ok_count_max_ = &this->count_ok_max_; arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
// arm_control_ptr->ng_count_max_ = &this->count_ng_max_; arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
// arm_control_ptr->startModeThread(); arm_control_ptr->startModeThread();
// } }
// else else
// { {
// arm_control_ptr->startHomeThread(); arm_control_ptr->startHomeThread();
// } }
// arm_control_ptr.reset(); arm_control_ptr.reset();
// ROS_INFO("Arm Finished"); ROS_INFO("Arm Finished");
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// this->arm_joined_ = false; this->arm_joined_ = false;
// } }
// void AmrController::unLoadCallBack() void AmrController::unLoadCallBack()
// { {
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// { {
// ROS_INFO("Shiping call"); ROS_INFO("Shiping call");
// this->belt_joined_ = true; this->belt_joined_ = true;
// this->cancel_ = false; this->cancel_ = false;
// this->cur_belt_state_en_ = amr_control::State::WAITING; this->cur_belt_state_en_ = amr_control::State::WAITING;
// this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_)); this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
// } }
// } }
// void AmrController::conveyorBeltsShipping(amr_control::State &state) void AmrController::conveyorBeltsShipping(amr_control::State &state)
// { {
// state = amr_control::State::INITIALIZING; state = amr_control::State::INITIALIZING;
// std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_; std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
// plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502); plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
// plc_controller_ptr_->connect(); plc_controller_ptr_->connect();
// if (!plc_controller_ptr_->checkConnected()) if (!plc_controller_ptr_->checkConnected())
// { {
// state = amr_control::State::FAILED; state = amr_control::State::FAILED;
// return; return;
// } }
// if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// { {
// int shipping_regsister = 119, receiving_regsister = 116; int shipping_regsister = 119, receiving_regsister = 116;
// plc_controller_ptr_->setM(shipping_regsister); plc_controller_ptr_->setM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister); plc_controller_ptr_->resetM(receiving_regsister);
// ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
// plc_controller_ptr_->resetM(shipping_regsister); plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister); plc_controller_ptr_->resetM(receiving_regsister);
// ros::Rate r(5); ros::Rate r(5);
// while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
// { {
// state = amr_control::State::RUNNING; state = amr_control::State::RUNNING;
// bool output_belt[2]; bool output_belt[2];
// plc_controller_ptr_->mulGetM(124, 125, output_belt); plc_controller_ptr_->mulGetM(124, 125, output_belt);
// bool enable_shipping = output_belt[0]; bool enable_shipping = output_belt[0];
// bool enable_receiving = output_belt[1]; bool enable_receiving = output_belt[1];
// if (!enable_shipping && !enable_receiving) if (!enable_shipping && !enable_receiving)
// { {
// state = amr_control::State::FINISHED; state = amr_control::State::FINISHED;
// break; break;
// } }
// r.sleep(); r.sleep();
// } }
// if (cancel_ || !plc_controller_ptr_->checkConnected()) if (cancel_ || !plc_controller_ptr_->checkConnected())
// state = amr_control::State::FAILED; state = amr_control::State::FAILED;
// plc_controller_ptr_->close(); plc_controller_ptr_->close();
// } }
// if (plc_controller_ptr_) if (plc_controller_ptr_)
// plc_controller_ptr_.reset(); plc_controller_ptr_.reset();
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// this->belt_joined_ = false; this->belt_joined_ = false;
// } }
// void AmrController::loadCallBack() void AmrController::loadCallBack()
// { {
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// { {
// ROS_INFO("Receiving call"); ROS_INFO("Receiving call");
// this->belt_joined_ = true; this->belt_joined_ = true;
// this->cancel_ = false; this->cancel_ = false;
// this->cur_belt_state_en_ = amr_control::State::WAITING; this->cur_belt_state_en_ = amr_control::State::WAITING;
// this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_)); this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
// } }
// } }
// void AmrController::conveyorBeltsReceiving(amr_control::State &state) void AmrController::conveyorBeltsReceiving(amr_control::State &state)
// { {
// state = amr_control::State::INITIALIZING; state = amr_control::State::INITIALIZING;
// std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_; std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
// plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502); plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
// plc_controller_ptr_->connect(); plc_controller_ptr_->connect();
// if (!plc_controller_ptr_->checkConnected()) if (!plc_controller_ptr_->checkConnected())
// { {
// state = amr_control::State::FAILED; state = amr_control::State::FAILED;
// return; return;
// } }
// if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// { {
// int shipping_regsister = 119, receiving_regsister = 116; int shipping_regsister = 119, receiving_regsister = 116;
// plc_controller_ptr_->resetM(shipping_regsister); plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->setM(receiving_regsister); plc_controller_ptr_->setM(receiving_regsister);
// ros::Duration(0.1).sleep(); ros::Duration(0.1).sleep();
// plc_controller_ptr_->resetM(shipping_regsister); plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister); plc_controller_ptr_->resetM(receiving_regsister);
// ros::Rate r(5); ros::Rate r(5);
// while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
// { {
// state = amr_control::State::RUNNING; state = amr_control::State::RUNNING;
// bool output_belt[2]; bool output_belt[2];
// plc_controller_ptr_->mulGetM(124, 125, output_belt); plc_controller_ptr_->mulGetM(124, 125, output_belt);
// bool enable_shipping = output_belt[0]; bool enable_shipping = output_belt[0];
// bool enable_receiving = output_belt[1]; bool enable_receiving = output_belt[1];
// if (!enable_shipping && !enable_receiving) if (!enable_shipping && !enable_receiving)
// { {
// state = amr_control::State::FINISHED; state = amr_control::State::FINISHED;
// break; break;
// } }
// r.sleep(); r.sleep();
// } }
// if (this->cancel_ || !plc_controller_ptr_->checkConnected()) if (this->cancel_ || !plc_controller_ptr_->checkConnected())
// state = amr_control::State::FAILED; state = amr_control::State::FAILED;
// plc_controller_ptr_->close(); plc_controller_ptr_->close();
// } }
// if (plc_controller_ptr_) if (plc_controller_ptr_)
// plc_controller_ptr_.reset(); plc_controller_ptr_.reset();
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// this->belt_joined_ = false; this->belt_joined_ = false;
// } }
// void AmrController::controllerDotuff() void AmrController::controllerDotuff()
// { {
// ros::Rate r(10); ros::Rate r(10);
// while (ros::ok()) while (ros::ok())
// { {
// std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_; std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
// plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502); plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
// plc_controller_ptr_->connect(); plc_controller_ptr_->connect();
// r.sleep(); r.sleep();
// ros::spinOnce(); ros::spinOnce();
// if (plc_controller_ptr_ == nullptr) if (plc_controller_ptr_ == nullptr)
// continue; continue;
// if (!plc_controller_ptr_->checkConnected()) if (!plc_controller_ptr_->checkConnected())
// continue; continue;
// this->amr_safety_ptr_ = std::make_shared<AmrSafety>(); this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
// this->amr_safety_ptr_->getController(plc_controller_ptr_); this->amr_safety_ptr_->getController(plc_controller_ptr_);
// while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// { {
// if (!this->monitor_ptr_) if (!this->monitor_ptr_)
// continue; continue;
// nav_2d_msgs::Twist2D velocity; nav_2d_msgs::Twist2D velocity;
// if (this->monitor_ptr_->getVelocity(velocity)) if (this->monitor_ptr_->getVelocity(velocity))
// { {
// cmd_vel_mtx.lock(); cmd_vel_mtx.lock();
// this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_); this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
// cmd_vel_mtx.unlock(); cmd_vel_mtx.unlock();
// } }
// if (move_base_ptr_ != nullptr && if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr && move_base_ptr_->getFeedback() != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready) move_base_ptr_->getFeedback()->is_ready)
// { {
// if (velocity.x <= -0.01) if (velocity.x <= -0.01)
// this->amr_safety_ptr_->writeMutesSafety(true); this->amr_safety_ptr_->writeMutesSafety(true);
// else else
// { {
// this->amr_safety_ptr_->writeMutesSafety(this->muted_); this->amr_safety_ptr_->writeMutesSafety(this->muted_);
// } }
// } }
// bool have_goods; bool have_goods;
// int have_goods_regsister = 142; int have_goods_regsister = 142;
// if (!OpcUAServerAPI::belt_cancel_) if (!OpcUAServerAPI::belt_cancel_)
// { {
// plc_controller_ptr_->getM(have_goods_regsister, have_goods); plc_controller_ptr_->getM(have_goods_regsister, have_goods);
// OpcUAServerAPI::have_goods_ = have_goods; OpcUAServerAPI::have_goods_ = have_goods;
// } }
// else else
// { {
// plc_controller_ptr_->resetM(have_goods_regsister); plc_controller_ptr_->resetM(have_goods_regsister);
// } }
// if (vda_5050_client_api_ptr_) if (vda_5050_client_api_ptr_)
// { {
// amr_control::OperatingMode mode_; amr_control::OperatingMode mode_;
// bool operating_mode[4]; bool operating_mode[4];
// plc_controller_ptr_->mulGetM(14, 17, operating_mode); plc_controller_ptr_->mulGetM(14, 17, operating_mode);
// if (operating_mode[0]) if (operating_mode[0])
// mode_ = amr_control::OperatingMode::AUTOMATIC; mode_ = amr_control::OperatingMode::AUTOMATIC;
// else if (operating_mode[2]) else if (operating_mode[2])
// mode_ = amr_control::OperatingMode::SERVICE; mode_ = amr_control::OperatingMode::SERVICE;
// else if (operating_mode[3]) else if (operating_mode[3])
// mode_ = amr_control::OperatingMode::MANUAL; mode_ = amr_control::OperatingMode::MANUAL;
// switch (mode_) switch (mode_)
// { {
// case amr_control::OperatingMode::AUTOMATIC: case amr_control::OperatingMode::AUTOMATIC:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC; vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
// break; break;
// case amr_control::OperatingMode::MANUAL: case amr_control::OperatingMode::MANUAL:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL; vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
// break; break;
// case amr_control::OperatingMode::SERVICE: case amr_control::OperatingMode::SERVICE:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
// break; break;
// default: default:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
// break; break;
// } }
// } }
// r.sleep(); r.sleep();
// ros::spinOnce(); ros::spinOnce();
// } }
// if (plc_controller_ptr_) if (plc_controller_ptr_)
// plc_controller_ptr_.reset(); plc_controller_ptr_.reset();
// if (amr_safety_ptr_) if (amr_safety_ptr_)
// amr_safety_ptr_.reset(); amr_safety_ptr_.reset();
// } }
// } }
// void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg) void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
// { {
// this->muted_ = msg->data; this->muted_ = msg->data;
// } }
// void AmrController::threadHandle() void AmrController::threadHandle()
// { {
// ros::Rate r(5); ros::Rate r(5);
// while (ros::ok()) while (ros::ok())
// { {
// if (this->arm_thread_.joinable()) if (this->arm_thread_.joinable())
// { {
// std::lock_guard<std::mutex> lock(this->arm_mutex_); std::lock_guard<std::mutex> lock(this->arm_mutex_);
// { {
// if (!this->arm_joined_) if (!this->arm_joined_)
// { {
// this->arm_thread_.join(); this->arm_thread_.join();
// } }
// } }
// } }
// if (this->belt_thread_.joinable()) if (this->belt_thread_.joinable())
// { {
// std::lock_guard<std::mutex> lock(this->belt_mutex_); std::lock_guard<std::mutex> lock(this->belt_mutex_);
// { {
// if (!this->belt_joined_) if (!this->belt_joined_)
// { {
// this->belt_thread_.join(); this->belt_thread_.join();
// } }
// } }
// } }
// if (move_base_ptr_ != nullptr && if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr && move_base_ptr_->getFeedback() != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready) move_base_ptr_->getFeedback()->is_ready)
// { {
// nav_2d_msgs::Twist2D velocity; nav_2d_msgs::Twist2D velocity;
// if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity)) if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
// { {
// this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x)); this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
// this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta); this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
// geometry_msgs::Vector3 linear; robot_geometry_msgs::Vector3 linear;
// geometry_msgs::Vector3 angular; robot_geometry_msgs::Vector3 angular;
// cmd_vel_mtx.lock(); cmd_vel_mtx.lock();
// linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x; linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
// angular.z = this->cmd_vel_max_.theta; angular.z = this->cmd_vel_max_.theta;
// cmd_vel_mtx.unlock(); cmd_vel_mtx.unlock();
// this->move_base_ptr_->setTwistLinear(linear); this->move_base_ptr_->setTwistLinear(linear);
// linear.x *= -1.0; linear.x *= -1.0;
// this->move_base_ptr_->setTwistLinear(linear); this->move_base_ptr_->setTwistLinear(linear);
// this->move_base_ptr_->setTwistAngular(angular); this->move_base_ptr_->setTwistAngular(angular);
// } }
// } }
// r.sleep(); r.sleep();
// ros::spinOnce(); ros::spinOnce();
// } }
// } }
} }

6597
Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
View File

File diff suppressed because it is too large Load Diff

145
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
View File

@ -20,7 +20,7 @@ namespace amr_control
this->count_ok_max_ = new unsigned int(1); this->count_ok_max_ = new unsigned int(1);
} }
VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<move_base_core::BaseNavigation> move_base, VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<robot::move_base_core::BaseNavigation> move_base,
std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<AmrMonitor> monitor) std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<AmrMonitor> monitor)
{ {
VDA5050ClientAPI::move_base_ptr_ = move_base; VDA5050ClientAPI::move_base_ptr_ = move_base;
@ -116,7 +116,7 @@ namespace amr_control
void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message) void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message)
{ {
geometry_msgs::PoseStamped goal; robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_) if (VDA5050ClientAPI::move_base_ptr_)
{ {
if (global_plan_msg_type_ == std::string("nav_msgs::Path")) if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@ -150,9 +150,9 @@ namespace amr_control
p_2d.theta = p.getYaw(); p_2d.theta = p.getYaw();
poses.push_back(p_2d); poses.push_back(p_2d);
} }
const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
goal = path.poses.back(); // goal = path.poses.back();
VDA5050ClientAPI::plan_pub_.publish(path); // VDA5050ClientAPI::plan_pub_.publish(path);
} }
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
{ {
@ -222,13 +222,13 @@ namespace amr_control
VDA5050ClientAPI::plan_pub_.publish(order_msg); VDA5050ClientAPI::plan_pub_.publish(order_msg);
vda_5050::NodePosition position = order.nodes.back().nodePosition; vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now(); goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map"; goal.header.frame_id = "map";
goal.pose.position.x = position.x; goal.pose.position.x = position.x;
goal.pose.position.y = position.y; goal.pose.position.y = position.y;
tf::Quaternion quat; // tf3::Transform transform;
quat.setRPY(0.0, 0.0, position.theta); // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
tf::quaternionTFToMsg(quat, goal.pose.orientation); // goal = tf3::toMsg(transform);
} }
else else
return; return;
@ -239,13 +239,13 @@ namespace amr_control
cmd_vel_max_.theta = 0.5; cmd_vel_max_.theta = 0.5;
cmd_vel_max_saved_ = cmd_vel_max_; cmd_vel_max_saved_ = cmd_vel_max_;
geometry_msgs::Vector3 linear; robot_geometry_msgs::Vector3 linear;
linear.x = cmd_vel_max_.x; linear.x = cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -cmd_vel_max_.x; linear.x = -cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
geometry_msgs::Vector3 angular; robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta; angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
@ -254,7 +254,7 @@ namespace amr_control
void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message) void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message)
{ {
geometry_msgs::PoseStamped goal; robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_) if (VDA5050ClientAPI::move_base_ptr_)
{ {
if (global_plan_msg_type_ == std::string("nav_msgs::Path")) if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@ -288,9 +288,9 @@ namespace amr_control
p_2d.theta = p.getYaw(); p_2d.theta = p.getYaw();
poses.push_back(p_2d); poses.push_back(p_2d);
} }
const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
goal = path.poses.back(); // goal = path.poses.back();
VDA5050ClientAPI::plan_pub_.publish(path); // VDA5050ClientAPI::plan_pub_.publish(path);
} }
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
{ {
@ -359,13 +359,13 @@ namespace amr_control
VDA5050ClientAPI::plan_pub_.publish(order_msg); VDA5050ClientAPI::plan_pub_.publish(order_msg);
vda_5050::NodePosition position = order.nodes.back().nodePosition; vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now(); goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map"; goal.header.frame_id = "map";
goal.pose.position.x = position.x; goal.pose.position.x = position.x;
goal.pose.position.y = position.y; goal.pose.position.y = position.y;
tf::Quaternion quat; // tf3::Transform transform;
quat.setRPY(0.0, 0.0, position.theta); // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
tf::quaternionTFToMsg(quat, goal.pose.orientation); // goal = tf3::toMsg(transform);
} }
else else
return; return;
@ -376,13 +376,13 @@ namespace amr_control
cmd_vel_max_.theta = 0.5; cmd_vel_max_.theta = 0.5;
cmd_vel_max_saved_ = cmd_vel_max_; cmd_vel_max_saved_ = cmd_vel_max_;
geometry_msgs::Vector3 linear; robot_geometry_msgs::Vector3 linear;
linear.x = cmd_vel_max_.x; linear.x = cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -cmd_vel_max_.x; linear.x = -cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
geometry_msgs::Vector3 angular; robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta; angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
@ -392,19 +392,18 @@ namespace amr_control
void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message) void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message)
{ {
geometry_msgs::PoseStamped goal; robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_) if (VDA5050ClientAPI::move_base_ptr_)
{ {
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal)) if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal))
{ {
vda_5050::NodePosition position = order.nodes.back().nodePosition; vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now(); goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map"; goal.header.frame_id = "map";
tf::Quaternion quat; // tf3::Transform transform;
quat.setRPY(0.0, 0.0, position.theta); // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
tf::quaternionTFToMsg(quat, goal.pose.orientation); // goal = tf3::toMsg(transform);
VDA5050ClientAPI::move_base_ptr_->rotateTo(goal); // VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
} }
else else
return; return;
@ -521,10 +520,10 @@ namespace amr_control
void VDA5050ClientAPI::updateVelocity(double velocity) void VDA5050ClientAPI::updateVelocity(double velocity)
{ {
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING) if (nav_state == robot::move_base_core::State::CONTROLLING)
{ {
if (fabs(velocity) > 2.0) if (fabs(velocity) > 2.0)
velocity = (fabs(velocity) / velocity) * 2.0; velocity = (fabs(velocity) / velocity) * 2.0;
@ -541,10 +540,10 @@ namespace amr_control
void VDA5050ClientAPI::updateAngular(double angular) void VDA5050ClientAPI::updateAngular(double angular)
{ {
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING) if (nav_state == robot::move_base_core::State::CONTROLLING)
{ {
if (fabs(angular) > 1.0) if (fabs(angular) > 1.0)
angular = (fabs(angular) / angular) * 1.0; angular = (fabs(angular) / angular) * 1.0;
@ -645,7 +644,7 @@ namespace amr_control
// Can runing actions be interrupted ? // Can runing actions be interrupted ?
if (VDA5050ClientAPI::move_base_ptr_ != nullptr && if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
VDA5050ClientAPI::move_base_ptr_->cancel(); VDA5050ClientAPI::move_base_ptr_->cancel();
*this->cancel_action_ = true; *this->cancel_action_ = true;
@ -653,7 +652,7 @@ namespace amr_control
*this->pause_action_ = false; *this->pause_action_ = false;
ros::Rate r(5); ros::Rate r(5);
while (ros::ok() && while (ros::ok() &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING)
{ {
ROS_INFO_THROTTLE(1.0, "Waiting to cancel"); ROS_INFO_THROTTLE(1.0, "Waiting to cancel");
r.sleep(); r.sleep();
@ -714,18 +713,18 @@ namespace amr_control
void VDA5050ClientAPI::unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state) void VDA5050ClientAPI::unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{ {
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
ros::Rate r(5); ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*this->cancel_action_ && nav_state == move_base_core::State::CONTROLLING) while (ros::ok() && !*this->cancel_action_ && nav_state == robot::move_base_core::State::CONTROLLING)
{ {
r.sleep(); r.sleep();
ros::spinOnce(); ros::spinOnce();
} }
geometry_msgs::PoseStamped pose; robot_geometry_msgs::PoseStamped pose;
if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose)) if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose))
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
@ -808,9 +807,9 @@ namespace amr_control
return; return;
} }
geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal); VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal);
geometry_msgs::Vector3 linear; robot_geometry_msgs::Vector3 linear;
linear.x = fabs(velocity); linear.x = fabs(velocity);
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -fabs(velocity); linear.x = -fabs(velocity);
@ -818,7 +817,7 @@ namespace amr_control
while (ros::ok() && !*this->cancel_action_) while (ros::ok() && !*this->cancel_action_)
{ {
if (nav_state == move_base_core::State::CONTROLLING) if (nav_state == robot::move_base_core::State::CONTROLLING)
break; break;
r.sleep(); r.sleep();
ros::spinOnce(); ros::spinOnce();
@ -827,15 +826,15 @@ namespace amr_control
while (ros::ok() && !*this->cancel_action_) while (ros::ok() && !*this->cancel_action_)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
if (nav_state == move_base_core::State::SUCCEEDED || if (nav_state == robot::move_base_core::State::SUCCEEDED ||
nav_state == move_base_core::State::ABORTED || nav_state == robot::move_base_core::State::ABORTED ||
nav_state == move_base_core::State::PREEMPTED) nav_state == robot::move_base_core::State::PREEMPTED)
break; break;
r.sleep(); r.sleep();
ros::spinOnce(); ros::spinOnce();
} }
if (nav_state == move_base_core::State::SUCCEEDED) if (nav_state == robot::move_base_core::State::SUCCEEDED)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
action_state->resultDescription = "Done"; action_state->resultDescription = "Done";
@ -851,11 +850,11 @@ namespace amr_control
void VDA5050ClientAPI::pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state) void VDA5050ClientAPI::pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{ {
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
ros::Rate r(5); ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
{ {
r.sleep(); r.sleep();
ros::spinOnce(); ros::spinOnce();
@ -978,18 +977,18 @@ namespace amr_control
void VDA5050ClientAPI::dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state) void VDA5050ClientAPI::dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{ {
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
ros::Rate r(5); ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
r.sleep(); r.sleep();
ros::spinOnce(); ros::spinOnce();
} }
if (nav_state == move_base_core::State::SUCCEEDED) if (nav_state == robot::move_base_core::State::SUCCEEDED)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
action_state->resultDescription = "Done"; action_state->resultDescription = "Done";
@ -1003,23 +1002,23 @@ namespace amr_control
void VDA5050ClientAPI::unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state) void VDA5050ClientAPI::unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{ {
ROS_INFO("UnLoad running"); ROS_INFO("UnLoad running");
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
ros::Rate r(5); ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*enable_action_ && !*this->cancel_action_) while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
{ {
if (nav_state == move_base_core::State::ABORTED || if (nav_state == robot::move_base_core::State::ABORTED ||
nav_state == move_base_core::State::REJECTED) nav_state == robot::move_base_core::State::REJECTED)
{ {
if (action_state != nullptr) if (action_state != nullptr)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
} }
return; return;
} }
if (nav_state == move_base_core::State::SUCCEEDED) if (nav_state == robot::move_base_core::State::SUCCEEDED)
break; break;
r.sleep(); r.sleep();
} }
@ -1081,23 +1080,23 @@ namespace amr_control
void VDA5050ClientAPI::load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state) void VDA5050ClientAPI::load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{ {
ROS_INFO("Load running"); ROS_INFO("Load running");
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
ros::Rate r(5); ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*enable_action_ && !*this->cancel_action_) while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
{ {
if (nav_state == move_base_core::State::ABORTED || if (nav_state == robot::move_base_core::State::ABORTED ||
nav_state == move_base_core::State::REJECTED) nav_state == robot::move_base_core::State::REJECTED)
{ {
if (action_state != nullptr) if (action_state != nullptr)
{ {
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
} }
return; return;
} }
if (nav_state == move_base_core::State::SUCCEEDED) if (nav_state == robot::move_base_core::State::SUCCEEDED)
break; break;
r.sleep(); r.sleep();
} }
@ -1185,7 +1184,7 @@ namespace amr_control
auto &global_visualization = this->client_ptr_->vda5050_visualization_; auto &global_visualization = this->client_ptr_->vda5050_visualization_;
if (VDA5050ClientAPI::move_base_ptr_) if (VDA5050ClientAPI::move_base_ptr_)
{ {
geometry_msgs::Pose2D robot_pose; robot_geometry_msgs::Pose2D robot_pose;
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
{ {
global_visualization.agvPosition.x = robot_pose.x; global_visualization.agvPosition.x = robot_pose.x;
@ -1211,9 +1210,9 @@ namespace amr_control
std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex); std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
auto &global_state = this->client_ptr_->vda5050_state_; auto &global_state = this->client_ptr_->vda5050_state_;
if (VDA5050ClientAPI::move_base_ptr_ != nullptr && if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{ {
geometry_msgs::Pose2D robot_pose; robot_geometry_msgs::Pose2D robot_pose;
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
{ {
global_state.agvPosition.x = robot_pose.x; global_state.agvPosition.x = robot_pose.x;
@ -1221,14 +1220,14 @@ namespace amr_control
global_state.agvPosition.theta = robot_pose.theta; global_state.agvPosition.theta = robot_pose.theta;
} }
move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; robot::move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING) if (nav_state == robot::move_base_core::State::CONTROLLING)
{ {
global_state.driving = true; global_state.driving = true;
global_state.paused = false; global_state.paused = false;
global_state.newBaseRequest = false; global_state.newBaseRequest = false;
} }
else if (nav_state == move_base_core::State::PAUSED) else if (nav_state == robot::move_base_core::State::PAUSED)
{ {
global_state.driving = false; global_state.driving = false;
global_state.paused = true; global_state.paused = true;
@ -1254,7 +1253,7 @@ namespace amr_control
*this->pause_action_ = false; *this->pause_action_ = false;
} }
std::shared_ptr<move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr; std::shared_ptr<robot::move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
std::shared_ptr<loc_core::BaseLocalization> VDA5050ClientAPI::loc_base_ptr_ = nullptr; std::shared_ptr<loc_core::BaseLocalization> VDA5050ClientAPI::loc_base_ptr_ = nullptr;
std::shared_ptr<AmrMonitor> VDA5050ClientAPI::monitor_ptr_ = nullptr; std::shared_ptr<AmrMonitor> VDA5050ClientAPI::monitor_ptr_ = nullptr;
} }

205
Controllers/Packages/amr_startup/CMakeLists.txt Normal file
View File

@ -0,0 +1,205 @@
cmake_minimum_required(VERSION 3.0.2)
project(amr_startup)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES amr_startup
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
# ${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/amr_startup.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/amr_startup_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY
config
launch
rviz
sdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_startup.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

48
Controllers/Packages/amr_startup/config/base_local_planner_params.yaml Normal file
View File

@ -0,0 +1,48 @@
#For full documentation of the parameters in this file, and a list of all the
#parameters available for TrajectoryPlannerROS, please see
#http://www.ros.org/wiki/base_local_planner
TrajectoryPlannerROS:
#Set the acceleration limits of the robot
acc_lim_th: 3.2
acc_lim_x: 0.5
acc_lim_y: 0.5
#Set the velocity limits of the robot
max_vel_x: 0.4
min_vel_x: 0.1
max_rotational_vel: 1.0
min_in_place_rotational_vel: 0.4
#The velocity the robot will command when trying to escape from a stuck situation
escape_vel: -0.2
#For this example, we'll use a holonomic robot
holonomic_robot: false
#Since we're using a holonomic robot, we'll set the set of y velocities it will sample
y_vels: [-0.3, -0.1, 0.1, 0.3]
#Set the tolerance on achieving a goal
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.05
#We'll configure how long and with what granularity we'll forward simulate trajectories
sim_time: 3.0
sim_granularity: 0.025
vx_samples: 5
vtheta_samples: 20
#Parameters for scoring trajectories
goal_distance_bias: 0.4
path_distance_bias: 0.7
occdist_scale: 0.3
heading_lookahead: 0.325
#We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example
dwa: false
#How far the robot must travel before oscillation flags are reset
oscillation_reset_dist: 0.01
#Eat up the plan as the robot moves along it
prune_plan: false

54
Controllers/Packages/amr_startup/config/costmap_common_params.yaml Executable file
View File

@ -0,0 +1,54 @@
robot_base_frame: base_footprint
transform_tolerance: 1.0
obstacle_range: 3.0
#mark_threshold: 1
publish_voxel_map: true
footprint_padding: 0.0
navigation_map:
map_topic: /map
map_pkg: managerments
map_file: maze
virtual_walls_map:
map_topic: /virtual_walls/map
namespace: /virtual_walls
map_pkg: managerments
map_file: maze
use_maximum: true
lethal_cost_threshold: 100
obstacles:
observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing
f_scan_marking:
topic: /f_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
f_scan_clearing:
topic: /f_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_marking:
topic: /b_scan
data_type: LaserScan
clearing: false
marking: true
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25
b_scan_clearing:
topic: /b_scan
data_type: LaserScan
clearing: true
marking: false
inf_is_valid: false
min_obstacle_height: 0.0
max_obstacle_height: 0.25

13
Controllers/Packages/amr_startup/config/costmap_global_params.yaml Executable file
View File

@ -0,0 +1,13 @@
global_costmap:
global_frame: map
update_frequency: 1.0
publish_frequency: 1.0
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.2
rolling_window: false
z_voxels: 10
inflation:
cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.

10
Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml Executable file
View File

@ -0,0 +1,10 @@
global_costmap:
frame_id: map
plugins:
- {name: navigation_map, type: "costmap_2d::StaticLayer" }
- {name: virtual_walls_map, type: "costmap_2d::StaticLayer" }
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles:
enabled: false
footprint_clearing_enabled: false

16
Controllers/Packages/amr_startup/config/costmap_local_params.yaml Executable file
View File

@ -0,0 +1,16 @@
local_costmap:
global_frame: odom
update_frequency: 5.0
publish_frequency: 5.0
rolling_window: true
raytrace_range: 2.0
resolution: 0.05
z_resolution: 0.15
z_voxels: 8
inflation:
cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius.
inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius.
width: 8.0
height: 8.0
origin_x: 0.0
origin_y: 0.0

8
Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml Executable file
View File

@ -0,0 +1,8 @@
local_costmap:
frame_id: odom
plugins:
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles:
enabled: true
footprint_clearing_enabled: true

17
Controllers/Packages/amr_startup/config/custom_global_params.yaml Normal file
View File

@ -0,0 +1,17 @@
base_global_planner: CustomPlanner
CustomPlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: false
nominalvel_mpersecs: 0.8
timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s
allow_unknown: true
directory_to_save_paths: "/init/paths"
pathway_filename: "pathway.txt"
current_pose_topic_name: "/amcl_pose"
map_frame_id: "map"
base_frame_id: "base_link"

55
Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml Executable file
View File

@ -0,0 +1,55 @@
base_local_planner: dwa_local_planner/DWAPlannerROS
DWAPlannerROS:
# Robot configuration
max_vel_x: 0.8
min_vel_x: -0.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_trans_vel: 0.8 # choose slightly less than the base's capability
min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
trans_stopped_vel: 0.03
# Warning!
# do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities
# are non-negligible and small in place rotational velocities will be created.
max_rot_vel: 1.0 # choose slightly less than the base's capability
min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity
rot_stopped_vel: 0.1
acc_lim_x: 1.5
acc_lim_y: 0.0 # diff drive robot
acc_limit_trans: 1.5
acc_lim_theta: 2.0
# Goal tolerance
yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide)
xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2
latch_xy_goal_tolerance: true
# Forward simulation
sim_time: 1.2
vx_samples: 15
vy_samples: 1 # diff drive robot, there is only one sample
vtheta_samples: 20
# Trajectory scoring
path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan
goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal
occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles
forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point
stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj.
scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint
max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed.
prune_plan: true
# Oscillation prevention
oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m
oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
global_frame_id: /odom # or <robot namespace>/odom

217
Controllers/Packages/amr_startup/config/ekf.yaml Executable file
View File

@ -0,0 +1,217 @@
# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
# computation until it receives at least one message from one of the inputs. It will then run continuously at the
# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
frequency: 50
# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
# filter will generate new output. Defaults to 1 / frequency if not specified.
sensor_timeout: 0.1
# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
# by, for example, an IMU. Defaults to false if unspecified.
two_d_mode: true
# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
# unspecified.
transform_time_offset: 0.0
# Use this parameter to specify how long the tf listener should wait for a transform to become available.
# Defaults to 0.0 if unspecified.
transform_timeout: 0.0
# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
# unhappy with any settings or data.
print_diagnostics: true
# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
# effects on the performance of the node. Defaults to false if unspecified.
debug: false
# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path.
debug_out_file: /path/to/debug/file.txt
# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
publish_tf: true
# Whether to publish the acceleration state. Defaults to false if unspecified.
publish_acceleration: false
# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
# ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
# Here is how to use the following settings:
# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
# odom_frame.
# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
# from landmark observations) then:
# 3a. Set your "world_frame" to your map_frame value
# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
# estimation node from robot_localization! However, that instance should *not* fuse the global data.
map_frame: map # Defaults to "map" if unspecified
odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified
base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified
world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified
# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
# default values, and must be specified.
odom0: odom
# Each sensor reading updates some or all of the filter's state. These options give you greater control over which
# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
# if unspecified, effectively making this parameter required for each sensor.
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
odom0_config: [false, false, false, # x y z
false, false, false, # roll pitch yaw
true, true, false, # vx vy vz
false, false, true, # vroll vpitch vyaw
false, false, false] # ax ay az
# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
# the size of the subscription queue so that more measurements are fused.
odom0_queue_size: 10
# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result
# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's
# algorithm.
odom0_nodelay: false
# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
# for twist measurements has no effect.
odom0_differential: false
# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
# for all future measurements. While you can achieve the same effect with the differential paremeter, the key
# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
odom0_relative: false
# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
# numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
# the thresholds.
#odom0_pose_rejection_threshold: 5
#odom0_twist_rejection_threshold: 1
# Further input parameter examples
# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html
imu0: imu_data
imu0_config: [false, false, false, # x y z
false, false, true, # roll pitch yaw
false, false, false, # vx vy vz
false, false, true, # vroll vpitch vyaw
true, false, false] # ax ay az
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 10
#imu0_pose_rejection_threshold: 0.8 # Note the difference in parameter names
#imu0_twist_rejection_threshold: 0.8 #
#imu0_linear_acceleration_rejection_threshold: 0.8 #
# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
imu0_remove_gravitational_acceleration: false
# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
# for the velocity variable in question, or decrease the variance of the variable in question in the measurement
# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
# predicition. Note that if an acceleration measurement for the variable in question is available from one of the
# inputs, the control term will be ignored.
# Whether or not we use the control input during predicition. Defaults to false.
use_control: false
# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
# false.
stamped_control: false
# The last issued control command will be used in prediction for this period. Defaults to 0.2.
control_timeout: 0.2
# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
control_config: [true, false, false, false, false, true]
# Places limits on how large the acceleration term will be. Should match your robot's kinematics.
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# Acceleration and deceleration limits are not always the same for robots.
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
# gains
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
# gains
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
# However, if users find that a given variable is slow to converge, one approach is to increase the
# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
# unspecified.
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
# question. Users should take care not to use large values for variables that will not be measured directly. The values
# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below
#if unspecified.
initial_estimate_covariance: [100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 10.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

36
Controllers/Packages/amr_startup/config/localization.yaml Normal file
View File

@ -0,0 +1,36 @@
Amcl:
use_map_topic: true
odom_model_type: "diff-corrected"
gui_publish_rate: 5.0
save_pose_rate: 0.5
laser_max_beams: 300
laser_min_range: -1.0
laser_max_range: -1.0
min_particles: 1000
max_particles: 3000
kld_err: 0.05
kld_z: 0.99
odom_alpha1: 0.02
odom_alpha2: 0.01
odom_alpha3: 0.01
odom_alpha4: 0.02
laser_z_hit: 0.5
laser_z_short: 0.05
laser_z_max: 0.05
laser_z_rand: 0.5
laser_sigma_hit: 0.2
laser_lambda_short: 0.1
laser_model_type: "likelihood_field"
laser_likelihood_max_dist: 1.0
update_min_d: 0.05
update_min_a: 0.05
odom_frame_id: odom
base_frame_id: base_footprint
global_frame_id: map
resample_interval: 1
transform_tolerance: 0.2
recovery_alpha_slow: 0.001
recovery_alpha_fast: 0.001
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_a: 0.0

120
Controllers/Packages/amr_startup/config/maker_sources.yaml Normal file
View File

@ -0,0 +1,120 @@
maker_sources: trolley charger dock_station undock_station
trolley:
plugins:
- {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""}
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
4legs:
maker_goal_frame: trolley_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 2.0
timeout: 60.0
vel_x: 0.1
vel_theta: 0.3
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
qrcode:
maker_goal_frame: qr_trolley
delay: 2.0
timeout: 60.0
vel_x: 0.05
vel_theta: 0.2
allow_rotate: true
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
charger:
plugins:
- {name: charger, docking_planner: "DockPlanner", docking_nav: ""}
charger:
maker_goal_frame: charger_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 2
timeout: 60
vel_x: 0.1
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
dock_station:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
station:
maker_goal_frame: dock_station_goal
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
delay: 2
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.01
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
dock_station_2:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
station:
maker_goal_frame: dock_station_goal_2
footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]]
delay: 2
timeout: 60
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.01
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
undock_station:
plugins:
- {name: station, docking_planner: "DockPlanner", docking_nav: ""}
- {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" }
station:
maker_goal_frame: undock_station_goal
footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
delay: 2.0
timeout: 60.0
vel_x: 0.15
vel_theta: 0.3
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36
qrcode:
maker_goal_frame: qr_station
delay: 2.0
timeout: 60.0
vel_x: 0.05
vel_theta: 0.2
allow_rotate: true
yaw_goal_tolerance: 0.01
xy_goal_tolerance: 0.01
min_lookahead_dist: 0.4
max_lookahead_dist: 1.0
lookahead_time: 1.5
angle_threshold: 0.36

63
Controllers/Packages/amr_startup/config/mapping.yaml Normal file
View File

@ -0,0 +1,63 @@
SlamToolBox:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
mode: mapping #localization
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 10.0
resolution: 0.05
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 14400.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true

24
Controllers/Packages/amr_startup/config/move_base_common_params.yaml Executable file
View File

@ -0,0 +1,24 @@
### replanning
controller_frequency: 20.0 # run controller at 15.0 Hz
controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan
planner_frequency: 0.0 # don't continually replan (only when controller failed)
planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s...
max_planning_retries: 5 # ... or after 10 attempts (whichever happens first)
oscillation_timeout: -1.0 # abort controller and trigger recovery behaviors after 30.0 s
oscillation_distance: 1.5
### recovery behaviors
recovery_behavior_enabled: false
recovery_behaviors: [
{name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
]
conservative_reset:
reset_distance: 3.0 # clear obstacles farther away than 3.0 m
invert_area_to_clear: true
aggressive_reset:
reset_distance: 3.0

106
Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml Executable file
View File

@ -0,0 +1,106 @@
base_local_planner: mpc_local_planner/MpcLocalPlannerROS
MpcLocalPlannerROS:
odom_topic: odom
## Robot settings
robot:
type: "unicycle"
unicycle:
max_vel_x: 0.5
max_vel_x_backwards: 0.3
max_vel_theta: 0.3
acc_lim_x: 0.4 # deactive bounds with zero
dec_lim_x: 0.4 # deactive bounds with zero
acc_lim_theta: 0.6 # deactivate bounds with zero
## Footprint model for collision avoidance
footprint_model:
type: "point"
is_footprint_dynamic: False
## Collision avoidance
collision_avoidance:
min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model
enable_dynamic_obstacles: False
force_inclusion_dist: 0.5
cutoff_dist: 2.0
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
collision_check_no_poses: 5
## Planning grid
grid:
type: "fd_grid"
grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist
dt_ref: 0.3 # and here the corresponding temporal resolution
xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below
warm_start: True
collocation_method: "forward_differences"
cost_integration_method: "left_sum"
variable_grid:
enable: False # We want a fixed grid
min_dt: 0.0;
max_dt: 10.0;
grid_adaptation:
enable: True
dt_hyst_ratio: 0.1
min_grid_size: 2
max_grid_size: 50
## Planning options
planning:
objective:
type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly
quadratic_form:
state_weights: [2.0, 2.0, 0.25]
control_weights: [0.1, 0.05]
integral_form: False
terminal_cost:
type: "quadratic" # can be "none"
quadratic:
final_state_weights: [10.0, 10.0, 0.5]
terminal_constraint:
type: "none" # can be "none"
l2_ball:
weight_matrix: [1.0, 1.0, 1.0]
radius: 5
## Controller options
controller:
outer_ocp_iterations: 1
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.04
global_plan_overwrite_orientation: False
global_plan_prune_distance: 1.5
allow_init_with_backward_motion: True
max_global_plan_lookahead_dist: 1.0 # Check horizon length
force_reinit_new_goal_dist: 1.0
force_reinit_new_goal_angular: 1.57
force_reinit_num_steps: 0
prefer_x_feedback: False
publish_ocp_results: True
## Solver settings
solver:
type: "ipopt"
ipopt:
iterations: 100
max_cpu_time: -1.0
ipopt_numeric_options:
tol: 1e-3
ipopt_string_options:
linear_solver: "mumps"
hessian_approximation: "exact" # exact or limited-memory
lsq_lm:
iterations: 10
weight_init_eq: 2
weight_init_ineq: 2
weight_init_bounds: 2
weight_adapt_factor_eq: 1.5
weight_adapt_factor_ineq: 1.5
weight_adapt_factor_bounds: 1.5
weight_adapt_max_eq: 500
weight_adapt_max_ineq: 500
weight_adapt_max_bounds: 500

280
Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m Executable file
View File

@ -0,0 +1,280 @@
% /*
% * Copyright (c) 2008, Maxim Likhachev
% * 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 Carnegie Mellon University 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 OWNER 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.
% */
function[] = genmprim_unicycle_highcost_5cm(outfilename)
%
%generates motion primitives and saves them into file
%
%written by Maxim Likhachev
%---------------------------------------------------
%
%defines
UNICYCLE_MPRIM_16DEGS = 1;
if UNICYCLE_MPRIM_16DEGS == 1
resolution = 0.05;
numberofangles = 16; %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7;
%multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1;
backwardcostmult = 40;
forwardandturncostmult = 2;
sidestepcostmult = 10;
turninplacecostmult = 20;
%note, what is shown x,y,theta changes (not absolute numbers)
%0 degreees
basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult];
basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult];
basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult];
%1/16 theta change
basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult];
basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult];
%turn in place
basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult];
%45 degrees
basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult];
basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult];
basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult];
basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult];
%turn in place
basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult];
%22.5 degrees
basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost)
%x aligned with the heading of the robot, angles are positive
%counterclockwise
%0 theta change
basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult];
basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult];
basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult];
%1/16 theta change
basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult];
basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult];
%turn in place
basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult];
basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult];
else
fprintf(1, 'ERROR: undefined mprims type\n');
return;
end;
fout = fopen(outfilename, 'w');
%write the header
fprintf(fout, 'resolution_m: %f\n', resolution);
fprintf(fout, 'numberofangles: %d\n', numberofangles);
fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles);
%iterate over angles
for angleind = 1:numberofangles
figure(1);
hold off;
text(0, 0, int2str(angleind));
%iterate over primitives
for primind = 1:numberofprimsperangle
fprintf(fout, 'primID: %d\n', primind-1);
fprintf(fout, 'startangle_c: %d\n', angleind-1);
%current angle
currentangle = (angleind-1)*2*pi/numberofangles;
currentangle_36000int = round((angleind-1)*36000/numberofangles);
%compute which template to use
if (rem(currentangle_36000int, 9000) == 0)
basemprimendpts_c = basemprimendpts0_c(primind,:);
angle = currentangle;
elseif (rem(currentangle_36000int, 4500) == 0)
basemprimendpts_c = basemprimendpts45_c(primind,:);
angle = currentangle - 45*pi/180;
elseif (rem(currentangle_36000int-7875, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well
angle = currentangle - 78.75*pi/180;
fprintf(1, '78p75\n');
elseif (rem(currentangle_36000int-6750, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well
%fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ...
% basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3));
angle = currentangle - 67.5*pi/180;
fprintf(1, '67p5\n');
elseif (rem(currentangle_36000int-5625, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y
basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1);
basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well
angle = currentangle - 56.25*pi/180;
fprintf(1, '56p25\n');
elseif (rem(currentangle_36000int-3375, 9000) == 0)
basemprimendpts_c = basemprimendpts33p75_c(primind,:);
angle = currentangle - 33.75*pi/180;
fprintf(1, '33p75\n');
elseif (rem(currentangle_36000int-2250, 9000) == 0)
basemprimendpts_c = basemprimendpts22p5_c(primind,:);
angle = currentangle - 22.5*pi/180;
fprintf(1, '22p5\n');
elseif (rem(currentangle_36000int-1125, 9000) == 0)
basemprimendpts_c = basemprimendpts11p25_c(primind,:);
angle = currentangle - 11.25*pi/180;
fprintf(1, '11p25\n');
else
fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int);
return;
end;
%now figure out what action will be
baseendpose_c = basemprimendpts_c(1:3);
additionalactioncostmult = basemprimendpts_c(4);
endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle));
endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle));
endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles);
endpose_c = [endx_c endy_c endtheta_c];
fprintf(1, 'rotation angle=%f\n', angle*180/pi);
if baseendpose_c(2) == 0 & baseendpose_c(3) == 0
%fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
end;
%generate intermediate poses (remember they are w.r.t 0,0 (and not
%centers of the cells)
numofsamples = 10;
intermcells_m = zeros(numofsamples,3);
if UNICYCLE_MPRIM_16DEGS == 1
startpt = [0 0 currentangle];
endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ...
rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles];
intermcells_m = zeros(numofsamples,3);
if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward
for iind = 1:numofsamples
intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ...
startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ...
0];
rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles);
intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi);
end;
else %unicycle-based move forward or backward
R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3));
sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))];
S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)];
l = S(1);
tvoverrv = S(2);
rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv);
tv = tvoverrv*rv;
if l < 0
fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l);
l = 0;
end;
%compute rv
%rv = baseendpose_c(3)*2*pi/numberofangles;
%compute tv
%tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
%tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
%tv = (tvx + tvy)/2.0;
%generate samples
for iind = 1:numofsamples
dt = (iind-1)/(numofsamples-1);
%dtheta = rv*dt + startpt(3);
%intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
% startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
% dtheta];
if(dt*tv < l)
intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ...
startpt(2) + dt*tv*sin(startpt(3)) ...
startpt(3)];
else
dtheta = rv*(dt - l/tv) + startpt(3);
intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ...
startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ...
dtheta];
end;
end;
%correct
errorxy = [endpt(1) - intermcells_m(numofsamples,1) ...
endpt(2) - intermcells_m(numofsamples,2)];
fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2));
interpfactor = [0:1/(numofsamples-1):1];
intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor';
intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor';
end;
end;
%write out
fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult);
fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1));
for interind = 1:size(intermcells_m, 1)
fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3));
end;
plot(intermcells_m(:,1), intermcells_m(:,2));
axis([-0.3 0.3 -0.3 0.3]);
text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3)));
hold on;
end;
grid;
pause;
end;
fclose('all');

416
Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py Executable file
View File

@ -0,0 +1,416 @@
#!/usr/bin/env python3
#
# Copyright (c) 2016, David Conner (Christopher Newport University)
# Based on genmprim_unicycle.m
# Copyright (c) 2008, Maxim Likhachev
# All rights reserved.
# converted by libermate utility (https://github.com/awesomebytes/libermate)
#
# 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 Carnegie Mellon University 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 OWNER 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.
import numpy as np
import rospkg
# if available import pylab (from matlibplot)
matplotlib_found = False
try:
import matplotlib.pylab as plt
matplotlib_found = True
except ImportError:
pass
def matrix_size(mat, elem=None):
if not elem:
return mat.shape
else:
return mat.shape[int(elem) - 1]
def genmprim_unicycle(outfilename, visualize=False, separate_plots=False):
visualize = matplotlib_found and visualize # Plot the primitives
# Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c,
# baseendpose_c, additionalactioncostmult, fout, numofsamples,
# basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename,
# numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult,
# rotation_angle, basemprimendpts_c, forwardandturncostmult,
# forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult,
# interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt,
# currentangle, numberofprimsperangle, resolution, currentangle_36000int,
# l, iind, errorxy, interind, endy_c, angleind, endpt
# Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text,
# int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen,
# round, size
# %
# %generates motion primitives and saves them into file
# %
# %written by Maxim Likhachev
# %---------------------------------------------------
# %
# %defines
UNICYCLE_MPRIM_16DEGS = 1.0
if UNICYCLE_MPRIM_16DEGS == 1.0:
resolution = 0.05
numberofangles = 16
# %preferably a power of 2, definitely multiple of 8
numberofprimsperangle = 7
# %multipliers (multiplier is used as costmult*cost)
forwardcostmult = 1.0
backwardcostmult = 40.0
forwardandturncostmult = 2.0
# sidestepcostmult = 10.0
turninplacecostmult = 20.0
# %note, what is shown x,y,theta changes (not absolute numbers)
# %0 degreees
basemprimendpts0_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult)))
basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult)))
basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %45 degrees
basemprimendpts45_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult)))
basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult)))
basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
# %22.5 degrees
basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4))
# %x,y,theta,costmult (multiplier is used as costmult*cost)
# %x aligned with the heading of the robot, angles are positive
# %counterclockwise
# %0 theta change
basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult)))
basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult)))
# %1/16 theta change
basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult)))
basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult)))
# %turn in place
basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult)))
basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult)))
else:
print('ERROR: undefined mprims type\n')
return []
fout = open(outfilename, 'w')
# %write the header
fout.write('resolution_m: %f\n' % (resolution))
fout.write('numberofangles: %d\n' % (numberofangles))
fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles))
# %iterate over angles
for angleind in np.arange(1.0, (numberofangles) + 1):
currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles
currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles)
if visualize:
if separate_plots:
fig = plt.figure(angleind)
plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0))
else:
fig = plt.figure(1)
plt.axis('equal')
plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution])
ax = fig.add_subplot(1, 1, 1)
major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution)
minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution)
ax.set_xticks(major_ticks)
ax.set_xticks(minor_ticks, minor=True)
ax.set_yticks(major_ticks)
ax.set_yticks(minor_ticks, minor=True)
ax.grid(which='minor', alpha=0.5)
ax.grid(which='major', alpha=0.9)
# %iterate over primitives
for primind in np.arange(1.0, (numberofprimsperangle) + 1):
fout.write('primID: %d\n' % (primind - 1))
fout.write('startangle_c: %d\n' % (angleind - 1))
# %current angle
# %compute which template to use
if (currentangle_36000int % 9000) == 0:
basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :]
angle = currentangle
elif (currentangle_36000int % 4500) == 0:
basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :]
angle = currentangle - 45.0 * np.pi / 180.0
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 7875) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts33p75_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (78.75 * np.pi) / 180.0
# print('78p75\n')
elif ((currentangle_36000int - 6750) % 9000) == 0:
basemprimendpts_c = (
1 * basemprimendpts22p5_c[int(primind) - 1, :]
) # 1* to force deep copy to avoid reference update below
basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1]
# %reverse x and y
basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0]
basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2]
# %reverse the angle as well
# print(
# '%d : %d %d %d onto %d %d %d\n'
# % (
# primind - 1,
# basemprimendpts22p5_c[int(primind) - 1, 0],
# basemprimendpts22p5_c[int(primind) - 1, 1],
# basemprimendpts22p5_c[int(primind) - 1, 2],
# basemprimendpts_c[0],
# basemprimendpts_c[1],
# basemprimendpts_c[2],
# )
# )
angle = currentangle - (67.5 * np.pi) / 180.0
print('67p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 5625) % 9000) == 0:
# basemprimendpts_c = (
# 1 * basemprimendpts11p25_c[primind, :]
# ) # 1* to force deep copy to avoid reference update below
# basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1]
# # %reverse x and y
# basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0]
# basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2]
# # %reverse the angle as well
# angle = currentangle - (56.25 * np.pi) / 180.0
# print('56p25\n')
# commented out because basemprimendpts33p75_c is undefined
# elif ((currentangle_36000int - 3375) % 9000) == 0:
# basemprimendpts_c = basemprimendpts33p75_c[int(primind), :]
# angle = currentangle - (33.75 * np.pi) / 180.0
# print('33p75\n')
elif ((currentangle_36000int - 2250) % 9000) == 0:
basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :]
angle = currentangle - (22.5 * np.pi) / 180.0
print('22p5\n')
# commented out because basemprimendpts11p25_c is undefined
# elif ((currentangle_36000int - 1125) % 9000) == 0:
# basemprimendpts_c = basemprimendpts11p25_c[int(primind), :]
# angle = currentangle - (11.25 * np.pi) / 180.0
# print('11p25\n')
else:
print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int)
return []
# %now figure out what action will be
baseendpose_c = basemprimendpts_c[0:3]
additionalactioncostmult = basemprimendpts_c[3]
endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle)))
endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle)))
endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)
endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c)))
print("endpose_c=", endpose_c)
print(('rotation angle=%f\n' % (angle * 180.0 / np.pi)))
# if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.):
# %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3));
# %generate intermediate poses (remember they are w.r.t 0,0 (and not
# %centers of the cells)
numofsamples = 10
intermcells_m = np.zeros((numofsamples, 3))
if UNICYCLE_MPRIM_16DEGS == 1.0:
startpt = np.array(np.hstack((0.0, 0.0, currentangle)))
endpt = np.array(
np.hstack(
(
(endpose_c[0] * resolution),
(endpose_c[1] * resolution),
(
((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi)
/ numberofangles
),
)
)
)
print("startpt =", startpt)
print("endpt =", endpt)
intermcells_m = np.zeros((numofsamples, 3))
if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0):
# %turn in place or move forward
for iind in np.arange(1.0, (numofsamples) + 1):
fraction = float(iind - 1) / (numofsamples - 1)
intermcells_m[int(iind) - 1, :] = np.array(
(
startpt[0] + (endpt[0] - startpt[0]) * fraction,
startpt[1] + (endpt[1] - startpt[1]) * fraction,
0,
)
)
rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles)
intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi))
# print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle
else:
# %unicycle-based move forward or backward (http://sbpl.net/node/53)
R = np.array(
np.vstack(
(
np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))),
np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))),
)
)
)
S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1]))))
l = S[0]
tvoverrv = S[1]
rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv
tv = tvoverrv * rv
# print "R=\n",R
# print "Rpi=\n",np.linalg.pinv(R)
# print "S=\n",S
# print "l=",l
# print "tvoverrv=",tvoverrv
# print "rv=",rv
# print "tv=",tv
if l < 0.0:
print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l)))
l = 0.0
# %compute rv
# %rv = baseendpose_c(3)*2*pi/numberofangles;
# %compute tv
# %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3)))
# %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3)))
# %tv = (tvx + tvy)/2.0;
# %generate samples
for iind in np.arange(1, numofsamples + 1):
dt = (iind - 1) / (numofsamples - 1)
# %dtheta = rv*dt + startpt(3);
# %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ...
# % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ...
# % dtheta];
if (dt * tv) < l:
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0] + dt * tv * np.cos(startpt[2]),
startpt[1] + dt * tv * np.sin(startpt[2]),
startpt[2],
)
)
)
else:
dtheta = rv * (dt - l / tv) + startpt[2]
intermcells_m[int(iind) - 1, :] = np.array(
np.hstack(
(
startpt[0]
+ l * np.cos(startpt[2])
+ tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])),
startpt[1]
+ l * np.sin(startpt[2])
- tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])),
dtheta,
)
)
)
# %correct
errorxy = np.array(
np.hstack(
(
endpt[0] - intermcells_m[int(numofsamples) - 1, 0],
endpt[1] - intermcells_m[int(numofsamples) - 1, 1],
)
)
)
# print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1]))
interpfactor = np.array(
np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1))))
)
# print "intermcells_m=",intermcells_m
# print "interp'=",interpfactor.conj().T
intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T
intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T
# %write out
fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2]))
fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult))
fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0)))
for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1):
fout.write(
'%.4f %.4f %.4f\n'
% (
intermcells_m[int(interind) - 1, 0],
intermcells_m[int(interind) - 1, 1],
intermcells_m[int(interind) - 1, 2],
)
)
if visualize:
plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o")
plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2]))
# if (visualize):
# plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time
fout.close()
if visualize:
# plt.waitforbuttonpress() # hold until buttom pressed
plt.show() # Keep windows open until the program is terminated
return []
if __name__ == "__main__":
rospack = rospkg.RosPack()
outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim'
genmprim_unicycle(outfilename, visualize=True)

1203
Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

2403
Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

1683
Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim Executable file
View File

File diff suppressed because it is too large Load Diff

8
Controllers/Packages/amr_startup/config/mqtt_general.yaml Normal file
View File

@ -0,0 +1,8 @@
MQTT:
Name: T801
Host: 172.20.235.170
Port: 1885
Client_ID: T801
Username: robotics
Password: robotics
Keep_Alive: 60

188
Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml Normal file
View File

@ -0,0 +1,188 @@
position_planner_name: PNKXLocalPlanner
docking_planner_name: PNKXDockingLocalPlanner
go_straight_planner_name: PNKXGoStraightLocalPlanner
rotate_planner_name: PNKXRotateLocalPlanner
base_local_planner: nav_core_adapter::LocalPlannerAdapter
yaw_goal_tolerance: 0.017
xy_goal_tolerance: 0.015
stateful: true
publish_topic: true
LocalPlannerAdapter:
PNKXLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXDockingLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXGoStraightLocalPlanner:
# Algorithm
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
algorithm_nav_name: mkt_algorithm::diff::GoStraight
# Goal checking
goal_checker_name: mkt_plugins::GoalChecker
PNKXRotateLocalPlanner:
# Algorithm
algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal
trajectory_generator_name: mkt_plugins::LimitedAccelGenerator
# Goal checking
goal_checker_name: mkt_plugins::SimpleGoalChecker
stateful: true
LimitedAccelGenerator:
max_vel_x: 1.2
min_vel_x: -1.2
max_vel_y: 0.0 # diff drive robot
min_vel_y: 0.0 # diff drive robot
max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability
min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity
max_vel_theta: 0.3 # max_rot_vel: 1.0 # choose slightly less than the base's capability
min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity
acc_lim_x: 1.0
acc_lim_y: 0.0 # diff drive robot
acc_lim_theta: 0.5
decel_lim_x: -2.0
decel_lim_y: -0.0
decel_lim_theta: -1.0
# Whether to split the path into segments or not
split_path: true
sim_time: 1.5
vx_samples: 15
vy_samples: 1
vtheta_samples: 10
discretize_by_time: true
angular_granularity: 0.05
linear_granularity: 0.1
# sim_period
include_last_point: true
PredictiveTrajectory:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.5
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
# stoped
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
# only when true:
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
# Inflation cost scaling (Limit velocity by proximity to obstacles)
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)
GoStraight:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
# Speed
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
# stoped
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
use_regulated_linear_velocity_scaling: false
use_cost_regulated_linear_velocity_scaling: false
RotateToGoal:
avoid_obstacles: false
xy_local_goal_tolerance: 0.01
angle_threshold: 0.6
index_samples: 60
follow_step_path: true
# Lookahead
use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false)
# only when false:
lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6)
# only when true:
min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3)
max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9)
lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5)
min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2)
max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2)
# Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true
use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true)
# only when true:
rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785)
# stoped
rot_stopped_velocity: 0.03
trans_stopped_velocity: 0.06
min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05)
# Regulated linear velocity scaling
use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true)
# only when true:
regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9)
regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25)
# Inflation cost scaling (Limit velocity by proximity to obstacles)
use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true)
inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0
cost_scaling_dist: 0.2 # (default: 0.6)
cost_scaling_gain: 2.0 # (default: 1.0)

71
Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml Executable file
View File

@ -0,0 +1,71 @@
# -----------------------------------
left_wheel : 'left_wheel_joint'
right_wheel : 'right_wheel_joint'
publish_rate: 50 # this is what the real MiR platform publishes (default: 50)
# These covariances are exactly what the real MiR platform publishes
pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0]
enable_odom_tf: true
enable_wheel_tf: true
allow_multiple_cmd_vel_publishers: true
# open_loop: false
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter.
# We don't set the value here because it's different for each MiR type (100, 250, ...), and
# the plugin figures out the correct values.
wheel_separation : 0.5106
wheel_radius : 0.1
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 1.0
# frame_ids (same as real MiR platform)
base_frame_id: base_footprint # default: base_link base_footprint
odom_frame_id: odom # default: odom
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : true
max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8
min_velocity : -1.5 # m/s
has_acceleration_limits: true
max_acceleration : 3.0 # m/s^2; move_base acc_lim_x: 1.5
min_acceleration : -3.0 # m/s^2
has_jerk_limits : true
max_jerk : 5.0 # m/s^3
angular:
z:
has_velocity_limits : true
max_velocity : 0.6 # rad/s; move_base max_rot_vel: 1.0
min_velocity : -0.6
has_acceleration_limits: true
max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0
has_jerk_limits : true
max_jerk : 2.5 # rad/s^3
left_wheel_joint:
lookup_name: WheelPlugin
max_publish_rate: 50
mesurement_topic: left_encoder
frame_id: left_wheel_link
wheel_topic: /left_wheel
subscribe_queue_size: 1
command_timeout: 5.0
origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id
right_wheel_joint:
lookup_name: WheelPlugin
max_publish_rate: 50
mesurement_topic: right_encoder
frame_id: right_wheel_link
wheel_topic: /right_wheel
subscribe_queue_size: 1
command_timeout: 5.0
origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id

10
Controllers/Packages/amr_startup/config/sbpl_global_params.yaml Executable file
View File

@ -0,0 +1,10 @@
base_global_planner: SBPLLatticePlanner
SBPLLatticePlanner:
environment_type: XYThetaLattice
planner_type: ARAPlanner
allocated_time: 10.0
initial_epsilon: 1.0
force_scratch_limit: 10000
forward_search: true
nominalvel_mpersecs: 0.2
timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s

61
Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml Normal file
View File

@ -0,0 +1,61 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (1/rate in seconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
f_mlse:
id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls"
subscribe_queue_size: 1
sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
#
b_mlse:
id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: mls/SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls"
subscribe_queue_size: 1
sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
#

3
Controllers/Packages/amr_startup/config/two_points_global_params.yaml Normal file
View File

@ -0,0 +1,3 @@
base_global_planner: TwoPointsPlanner
TwoPointsPlanner:
lethal_obstacle: 20

50
Controllers/Packages/amr_startup/launch/amr_control.launch Normal file
View File

@ -0,0 +1,50 @@
<launch>
<arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
<arg name="local_planner" default="pnkx" doc="Local planner can be either dwa, dwb, eband, base, teb or pose" />
<arg name="global_planner" default="custom" doc="Global planner can be either sbpl two_points custom"/>
<arg name="global_plan_msg_type" default="vda5050_msgs::Order" doc="nav_msgs::Path or vda5050_msgs::Order"/>
<arg name="primitive_filename" default="$(find amr_startup)/config/mprim/unicycle_highcost_5cm.mprim"/>
<arg name="with_virtual_walls" default="true" doc="Enables usage of virtual walls when set. Set to false when running SLAM." />
<arg name="prefix" default="" doc="Prefix used for robot tf frames" /> <!-- used in the config files -->
<param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
<rosparam file="$(find amr_startup)/config/mqtt_general.yaml" command="load" />
<node pkg="amr_control" type="amr_control_node" respawn="false" name="amr_node" output="screen" clear_params="true">
<!-- robot type -->
<rosparam param="footprint" if="$(eval robot_type == 'imr')">
[[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]]
</rosparam>
<!-- Footprint cho imr -->
<rosparam param="footprint" if="$(eval robot_type == 'hook_150')">
[[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]]
</rosparam>
<rosparam param="footprint" if="$(eval robot_type == 'T800')">
[[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]]
</rosparam>
<param name="primitive_filename" value="$(arg primitive_filename)" />
<param name="global_plan_msg_type" value="$(arg global_plan_msg_type)" />
<rosparam file="$(find amr_startup)/config/maker_sources.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/move_base_common_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg global_planner)_global_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/$(arg local_planner)_local_planner_params.yaml" command="load" />
<!-- global costmap params -->
<rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="global_costmap" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_global_params.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/costmap_global_params_plugins_no_virtual_walls.yaml" command="load" />
<!-- local costmap params -->
<rosparam file="$(find amr_startup)/config/costmap_common_params.yaml" command="load" ns="local_costmap" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_local_params.yaml" command="load" subst_value="true" />
<rosparam file="$(find amr_startup)/config/costmap_local_params_plugins_no_virtual_walls.yaml" command="load" />
<rosparam file="$(find amr_startup)/config/localization.yaml" command="load" />
<remap from="map" to="/map" />
<remap from="marker" to="move_base_node/markers" />
<!-- <remap from="cmd_vel" to="/cmd_vel_1" /> -->
</node>
</launch>

102
Controllers/Packages/amr_startup/launch/amr_startup.launch Normal file
View File

@ -0,0 +1,102 @@
<?xml version="1.0"?>
<launch>
<arg name="tf_prefix" default="" />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="imu_link_to_base_link" args="0 0 0.45 0 0 0 base_link imu_link" />
<node pkg="tf2_ros" type="static_transform_publisher" name="surface_to_base_link" args="0 0 0.9 0 0 0 base_link surface" />
<include file="$(find sick_line_guidance)/launch/sick_line_guidance.launch">
<arg name="yaml" value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
</include>
<include file="$(find mlse_tf_base_link)/launch/msle_tf_base_link.launch">
<arg name="yaml" value="$(find amr_startup)/config/sick_line_guidance_mls.yaml"/>
</include>
<node pkg="diff_wheel_plugin" type="diff_wheel_feedback" name="diff_wheel_feedback" output="screen">
<rosparam file="$(find amr_startup)/config/plc_config.yaml" command="load" />
<param name="start_bit" value="111"/>
<param name="end_bit" value="114"/>
</node>
<node pkg="diff_wheel_plugin" type="diff_wheel_controller" name="diff_wheel_controller" output="screen">
<rosparam file="$(find amr_startup)/config/motorInfomation.yaml" command="load" />
<param name="port_name" type="str" value="/dev/ttyTHS0"/>
</node>
<arg name="frame_id" default="imu_frame"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg frame_id)_to_imu_link" args="0 0 0 0 0 0 imu_link $(arg frame_id)" />
<node pkg="wit_wt901ble_reader" name="wit_wt901ble_reader_node" type="wit_wt901ble_reader_node" output="screen">
<param name="baudrate" type="int" value="115200"/>
<param name="portname" type="string" value="/dev/USB_IMU"/>
<param name="topic_name" type="string" value="/imu_data"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="imu_diagnostics_topic_name" type="string" value="/imu_diagnostics"/>
</node>
<node pkg="ros_kinematics" type="ros_kinematics_node" name="ros_kinematics" output="screen">
<rosparam file="$(find amr_startup)/config/ros_diff_drive_controller.yaml" command="load" />
<param name="use_encoder" type="bool" value="false"/>
<param name="type" type="int" value="2"/>
<remap from="/ros_kinematics/odom" to="/odom" />
</node>
<!-- <include file="$(find manual_psx)/launch/ps4.launch"></include> -->
<!-- <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
<rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
</node> -->
<!-- Load URDF -->
<include file="$(find robot_description)/launch/upload_urdf.launch">
<arg name="robot_type" value="imr" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
<!-- driver -->
<arg name="pkg" value="olelidar"/>
<arg name="driver" default="true"/>
<arg name="f_device_ip" default="192.168.2.101"/>
<arg name="f_device_port" default="2368"/>
<arg name="local_ip" default="192.168.2.100"/>
<arg name="multiaddr" default=""/>
<!-- decoder -->
<arg name="f_frame_id" default="front_laser_link"/>
<arg name="f_topic_name" default="/f_scan"/>
<arg name="f_r_max" default="25"/>
<arg name="f_ang_start" default="-90"/>
<arg name="f_ang_end" default="90"/>
<arg name="decoder" default="true"/>
<arg name="inverted" default="false"/>
<arg name="f_debug" default="false"/>
<env if="$(arg f_debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="$(arg f_frame_id)_to_base_link" args="0.432 0 0.135 0 0 0 base_link $(arg f_frame_id)" />
<node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
<param name="frame_id" type="string" value="$(arg f_frame_id)"/>
<param name="r_max" type="int" value="$(arg f_r_max)"/>
<param name="ang_start" type="int" value="$(arg f_ang_start)"/>
<param name="ang_end" type="int" value="$(arg f_ang_end)"/>
<param name="inverted" type="bool" value="$(arg inverted)"/>
<param name="device_ip" type="string" value="$(arg f_device_ip)"/>
<param name="device_port" type="int" value="$(arg f_device_port)"/>
<param name="local_ip" type="string" value="$(arg local_ip)"/>
<param name="multiaddr" type="string" value="$(arg multiaddr)"/>
<remap from="~packet" to="packet"/>
<remap from="~scan" to="$(arg f_topic_name)"/>
</node>
<node pkg="topic_tools" type="relay" name="f_scan_relay" args="$(arg f_topic_name) scan"/>
<include file="$(find amr_startup)/launch/amr_control.launch"></include>
<!-- <include file="$(find amr_startup)/launch/includes/rviz.launch"></include> -->
<!-- <include file="$(find slam_toolbox)/launch/offline.launch"></include> -->
</launch>

5
Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch Executable file
View File

@ -0,0 +1,5 @@
<launch>
<!-- <node pkg="tf" type="static_transform_publisher" name="goal_charger_broadcaster" args="1.9 0.07 0 0.0 0 0 base_link shelf_target 100" /> -->
<node pkg="tf" type="static_transform_publisher" name="base_link_surface_broadcaster" args="0 0 0 0 0 0 base_link surface 1000" />
</launch>

65
Controllers/Packages/amr_startup/launch/hector_mapping.launch Normal file
View File

@ -0,0 +1,65 @@
<?xml version="1.0"?>
<launch>
<arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="odom"/>
<arg name="pub_map_odom_transform" default="true"/>
<arg name="scan_subscriber_queue_size" default="5"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_size" default="2048"/>
<!-- set use_tf_pose_start_estimate and map_with_known_poses to `true` when
the map-base_footprint transform is provided by a different node, such
as fake_localization -->
<arg name="disable_localization" default="false"/>
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
<!-- Frame names -->
<param name="map_frame" value="map" />
<param name="base_frame" value="$(arg base_frame)" />
<param name="odom_frame" value="$(arg odom_frame)" />
<!-- Tf use -->
<param name="use_tf_scan_transformation" value="true"/>
<param name="use_tf_pose_start_estimate" value="$(arg disable_localization)"/>
<param name="map_with_known_poses" value="$(arg disable_localization)" />
<param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>
<param name="pub_map_scanmatch_transform" value="true" />
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="$(arg map_size)"/>
<param name="map_start_x" value="0.5"/>
<param name="map_start_y" value="0.5" />
<param name="map_multi_res_levels" value="3" />
<param name="map_pub_period" value ="2.0" />
<!-- Map update parameters -->
<param name="update_factor_free" value="0.4"/>
<param name="update_factor_occupied" value="0.9" />
<param name="map_update_distance_thresh" value="0.4"/>
<param name="map_update_angle_thresh" value="0.06" />
<param name="laser_min_dist" value="0.1" />
<param name="laser_max_dist" value="10.0" />
<param name="laser_z_min_value" value="-1.0" />
<param name="laser_z_max_value" value="1.0" />
<!-- Advertising config -->
<param name="advertise_map_service" value="true"/>
<param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
<param name="scan_topic" value="$(arg scan_topic)"/>
<param name="pose_update_topic" value="poseupdate" />
<param name="sys_msg_topic" value="syscommand" />
<param name="pub_odometry" value="false" />
<!-- Debug parameters -->
<!--
<param name="output_timing" value="false"/>
<param name="pub_drawings" value="true"/>
<param name="pub_debug_output" value="true"/>
-->
<param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
<remap from="map" to="/map" />
</node>
</launch>

105
Controllers/Packages/amr_startup/launch/includes/amcl.launch Normal file
View File

@ -0,0 +1,105 @@
<?xml version="1.0" ?>
<launch>
<arg name="tf_prefix" default="" />
<arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
<arg name="use_map_topic" default="true"/>
<arg name="scan_topic" default="scan"/>
<arg name="map_topic" default="/map"/> <!-- if use_map_topic = true -->
<arg name="map_service" default="/static_map"/> <!-- if use_map_topic = false -->
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="$(arg tf_prefix)/odom"/>
<arg name="base_frame_id" default="$(arg tf_prefix)/base_footprint"/>
<arg name="global_frame_id" default="/map"/>
<group if="$(eval namespace != '')" ns="$(arg namespace)">
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="200"/>
<param name="laser_min_range" value="-1.0"/>
<param name="laser_max_range" value="-1.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.09"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.01"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.02"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.2"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="map" to="$(arg map_topic)"/>
<remap from="static_map" to="$(arg map_service)"/>
</node>
</group>
<!-- Duplicate of the above in case namespace is empty. This is necessary to
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
<group unless="$(eval namespace != '')">
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="200"/>
<param name="laser_min_range" value="-1.0"/>
<param name="laser_max_range" value="-1.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.09"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.02"/>
<param name="odom_alpha2" value="0.01"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.02"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.2"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="map" to="$(arg map_topic)"/>
<remap from="static_map" to="$(arg map_service)"/>
</node>
</group>
</launch>

7
Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml Normal file
View File

@ -0,0 +1,7 @@
<?xml version="1.0"?>
<launch>
<arg name="tf_prefix" default="" />
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization_node" clear_params="true" output="screen">
<rosparam command="load" file="$(find amr_startup)/config/ekf.yaml" subst_value="true" />
</node>
</launch>

3
Controllers/Packages/amr_startup/launch/includes/rviz.launch Normal file
View File

@ -0,0 +1,3 @@
<launch>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
</launch>

5
Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<launch>
<node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" args="-sdf
-file $(find amr_startup)/sdf/maze/model.sdf -model walls" output="screen" />
</launch>

11
Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<arg name="robot_x" default="17.5" />
<arg name="robot_y" default="34.5" />
<arg name="robot_yaw" default="1.57079" />
<arg name="model" default="trolley" />
<node name="spawn_maze" pkg="gazebo_ros" type="spawn_model"
args="-sdf -file $(find factory_ss_demo)models/sehc_trolley/model.sdf -model $(arg model)
-x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
</launch>

72
Controllers/Packages/amr_startup/launch/robot_empty_world.launch Normal file
View File

@ -0,0 +1,72 @@
<?xml version="1.0" ?>
<launch>
<arg name="gui" default="true" />
<arg name="world_name" default="worlds/empty.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable, but can also be an absolute path -->
<arg name="robot_x" default="0.0" />
<arg name="robot_y" default="0.0" />
<arg name="robot_yaw" default="0.0" />
<arg name="robot_type" default="hook_150" doc="Can be 'hook_150' or 'imr' for now." />
<arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
<arg name="namespace" default="$(arg tf_prefix)" doc="Namespace to push all topics into."/>
<group if="$(eval namespace != '')">
<group>
<remap from="$(arg namespace)/joint_states" to="$(arg namespace)/robot/joint_states" />
<remap from="$(arg namespace)/mobile_base_controller/cmd_vel" to="$(arg namespace)/cmd_vel" />
<remap from="$(arg namespace)/mobile_base_controller/odom" to="$(arg namespace)/odom" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="debug" default="false" />
<arg name="verbose" default="true" />
</include>
</group>
<group ns="$(arg namespace)">
<!-- spawn robot and bring up controllers etc. -->
<include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
<arg name="robot_x" value="$(arg robot_x)" />
<arg name="robot_y" value="$(arg robot_y)" />
<arg name="robot_yaw" value="$(arg robot_yaw)" />
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
</group>
</group>
<!-- Duplicate of the above in case namespace is empty. This is necessary to
avoid the "<group> tag has an empty 'ns' attribute" parsing error. -->
<group unless="$(eval namespace != '')">
<group>
<remap from="joint_states" to="robot/joint_states" />
<remap from="mobile_base_controller/cmd_vel" to="cmd_vel" />
<remap from="mobile_base_controller/odom" to="odom" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="debug" default="false" />
<arg name="verbose" default="true" />
</include>
</group>
<include file="$(find amr_startup)/launch/robot_gazebo_common.launch">
<arg name="robot_x" value="$(arg robot_x)" />
<arg name="robot_y" value="$(arg robot_y)" />
<arg name="robot_yaw" value="$(arg robot_yaw)" />
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
</group>
</launch>

59
Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_x" default="0.0" />
<arg name="robot_y" default="0.0" />
<arg name="robot_yaw" default="0.0" />
<arg name="robot_type" default="hool_150" doc="Can be 'hool_150' or 'imr' for now." />
<arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
<arg name="prefix" value="$(arg tf_prefix)/" if="$(eval tf_prefix != '')" /> <!-- $(arg prefix) is used in all the config files! TODO: For multiple robots, create groups when loading the parameters to overwrite the arg? -->
<arg name="prefix" value="" unless="$(eval tf_prefix != '')" />
<arg name="model_name" default="robot" doc="Name of the Gazebo robot model (needs to be different for each robot)" />
<param name="rosconsole_config_file" value="$(find amr_startup)/rosconsole.config"/>
<!-- Load URDF -->
<include file="$(find robot_description)/launch/upload_urdf.launch">
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
<!-- Spawn the robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg model_name)
-x $(arg robot_x) -y $(arg robot_y) -Y $(arg robot_yaw)" output="screen" />
<!-- Load ros_control controller configurations -->
<rosparam file="$(find robot_description)/config/joint_state_controller.yaml" command="load" />
<rosparam file="$(find robot_description)/config/diffdrive_controller.yaml" command="load" subst_value="true" />
<rosparam file="$(find robot_description)/config/pid_gains.yaml" command="load" />
<!-- Start the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" output="screen"
args="joint_state_controller mobile_base_controller"/>
<!-- Add passive + mimic joints to joint_states topic -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<rosparam param="source_list">[robot/joint_states]</rosparam>
<param name="rate" value="200.0" />
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen" />
<node name="rqt_robot_steering" pkg="rqt_robot_steering" type="rqt_robot_steering">
<param name="default_topic" value="cmd_vel"/>
<param name="default_vx_max" value="1.0" />
<param name="default_vx_min" value="-1.0" />
<param name="default_vw_max" value="1.5" />
<param name="default_vw_min" value="-1.5" />
</node>
<!-- EKF -->
<include file="$(find amr_startup)/launch/includes/ekf.launch.xml">
<arg name="tf_prefix" value="$(arg prefix)" />
</include>
<!-- create combined scan topic (like on robot real) -->
<node pkg="topic_tools" type="relay" name="b_scan_relay" args="b_scan scan"/>
<node pkg="topic_tools" type="relay" name="f_scan_relay" args="f_scan scan"/>
</launch>

15
Controllers/Packages/amr_startup/launch/robot_maze_world.launch Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<launch>
<arg name="gui" default="true" />
<arg name="robot_type" default="T800" doc="Can be 'hook_150' or 'imr' for now." />
<arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
<include file="$(find amr_startup)/launch/robot_empty_world.launch">
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
<include file="$(find amr_startup)/launch/includes/spawn_maze.launch.xml" />
<!-- <include file="$(find amr_startup)/launch/includes/rviz.launch" /> -->
</launch>

18
Controllers/Packages/amr_startup/launch/sehc_world.launch Normal file
View File

@ -0,0 +1,18 @@
<launch>
<arg name="gui" default="true" />
<arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
<arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
<include file="$(find amr_startup)/launch/robot_empty_world.launch">
<arg name='world_name' value="$(find factory_ss_demo)/worlds/factory_ss_demo.world"/>
<arg name="robot_x" value="5.5"/>
<arg name="robot_y" value="34.5"/>
<arg name="robot_yaw" value="0.0"/>
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
<include file="$(find amr_startup)/launch/includes/spawn_trolley.launch"/>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="false" />
</launch>

14
Controllers/Packages/amr_startup/launch/start_maps.launch Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<launch>
<arg name="map_file" default="$(find managerments)/maps/maze.yaml" doc="Path to a map .yaml file (required)." />
<arg name="virtual_walls_map_file" default="$(arg map_file)" doc="Path to a virtual walls map .yaml file (optional)." />
<arg name="with_virtual_walls" default="true" />
<node name="static_map_server" pkg="map_server" type="map_server" args="$(arg map_file)" ns="/" output="screen">
<param name="frame_id" type="string" value="map"/>
</node>
<node if="$(arg with_virtual_walls)" name="virtual_walls_map_server" pkg="map_server" type="map_server" args="$(arg virtual_walls_map_file)" ns="/virtual_walls" output="screen">
<param name="frame_id" type="string" value="map"/>
</node>
</launch>

16
Controllers/Packages/amr_startup/launch/ware_houses_world.launch Normal file
View File

@ -0,0 +1,16 @@
<launch>
<arg name="gui" default="true" />
<arg name="robot_type" default="imr" doc="Can be 'hook_150' or 'imr' for now." />
<arg name="tf_prefix" default="" doc="tf_prefix to be used by gazebo plugins and in the robot's urdf etc." />
<include file="$(find amr_startup)/launch/robot_empty_world.launch">
<arg name='world_name' value="$(find dynamic_logistics_warehouse)/worlds/warehouse.world"/>
<arg name="robot_x" value="12.0"/>
<arg name="robot_y" value="22.0"/>
<arg name="robot_yaw" value="3.141592654"/>
<arg name="gui" value="$(arg gui)" />
<arg name="robot_type" value="$(arg robot_type)" />
<arg name="tf_prefix" value="$(arg tf_prefix)" />
</include>
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find amr_startup)/rviz/navigation.rviz" required="true" />
</launch>

59
Controllers/Packages/amr_startup/package.xml Normal file
View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<package format="2">
<name>amr_startup</name>
<version>0.0.0</version>
<description>The amr_startup package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/amr_startup</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

2
Controllers/Packages/amr_startup/rosconsole.config Normal file
View File

@ -0,0 +1,2 @@
log4j.logger.ros.tf=ERROR
log4j.logger.ros.tf2=ERROR

646
Controllers/Packages/amr_startup/rviz/navigation.rviz Normal file
View File

@ -0,0 +1,646 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Grid1/Offset1
- /TF1/Frames1
- /Global Map1/Straight Path1
- /Local Map1
- /Local Map1/Trajectory1
- /Local Map1/Trajectory1/transform plan1
- /Pose1
Splitter Ratio: 0.37295082211494446
Tree Height: 422
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: Back LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 5
Z: 0
Plane: XY
Plane Cell Count: 100
Reference Frame: map
Value: true
- Alpha: 0.4000000059604645
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic: /map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 0.5
Class: rviz/RobotModel
Collision Enabled: true
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: false
Visual Enabled: true
- Class: rviz/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: false
back_nanoscan3_base_link:
Value: false
back_nanoscan3_sensor_link:
Value: false
base_footprint:
Value: true
base_link:
Value: false
bl_caster_rotation_link:
Value: false
bl_caster_wheel_link:
Value: false
br_caster_rotation_link:
Value: false
br_caster_wheel_link:
Value: false
camera_link:
Value: false
fl_caster_rotation_link:
Value: false
fl_caster_wheel_link:
Value: false
fr_caster_rotation_link:
Value: false
fr_caster_wheel_link:
Value: false
front_nanoscan3_base_link:
Value: false
front_nanoscan3_sensor_link:
Value: false
imu_frame:
Value: false
imu_link:
Value: false
left_wheel_link:
Value: false
lifting_link:
Value: false
map:
Value: true
odom:
Value: true
right_wheel_link:
Value: false
surface:
Value: false
Marker Alpha: 1
Marker Scale: 0.5
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
map:
odom:
base_footprint:
base_link:
back_nanoscan3_base_link:
back_nanoscan3_sensor_link:
{}
bl_caster_rotation_link:
bl_caster_wheel_link:
{}
br_caster_rotation_link:
br_caster_wheel_link:
{}
camera_link:
{}
fl_caster_rotation_link:
fl_caster_wheel_link:
{}
fr_caster_rotation_link:
fr_caster_wheel_link:
{}
front_nanoscan3_base_link:
front_nanoscan3_sensor_link:
{}
imu_link:
imu_frame:
{}
left_wheel_link:
{}
lifting_link:
surface:
{}
right_wheel_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 204; 0; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: Front LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 6
Size (m): 0.09000000357627869
Style: Points
Topic: f_scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: Back LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /b_scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Arrow Length: 0.20000000298023224
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz/PoseArray
Color: 0; 192; 0
Enabled: false
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: Amcl Particle Swarm
Queue Size: 10
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic: particlecloud
Unreliable: false
Value: false
- Class: rviz/Group
Displays:
- Alpha: 0.5
Class: rviz/Map
Color Scheme: costmap
Draw Behind: true
Enabled: true
Name: Costmap
Topic: /amr_node/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.20000000298023224
Line Style: Lines
Line Width: 0.05000000074505806
Name: Full Plan
Offset:
X: 0
Y: 0
Z: 0.8999999761581421
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.019999999552965164
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/SBPLLatticePlanner/plan
Unreliable: false
Value: false
- Alpha: 1
Class: rviz/Polygon
Color: 255; 0; 0
Enabled: true
Name: Footprint
Queue Size: 10
Topic: move_base_node/global_costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.00800000037997961
Line Style: Billboards
Line Width: 0.009999999776482582
Name: CustomPath
Offset:
X: 0
Y: 0
Z: 0.8999999761581421
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.004999999888241291
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/CustomPlanner/plan
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 0; 0; 0
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Straight Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan
Unreliable: false
Value: false
Enabled: true
Name: Global Map
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 252; 175; 62
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.03999999910593033
Line Style: Lines
Line Width: 0.029999999329447746
Name: Global Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.009999999776482582
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/global_plan
Unreliable: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Costmap
Topic: /amr_node/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Class: rviz/Polygon
Color: 25; 255; 0
Enabled: true
Name: Footprint
Queue Size: 10
Topic: /amr_node/local_costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 138; 226; 52
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.029999999329447746
Line Style: Billboards
Line Width: 0.029999999329447746
Name: Local Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.009999999776482582
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan
Unreliable: false
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /amr_node/PredictiveTrajectory/cost_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
Axes Length: 0.10000000149011612
Axes Radius: 0.029999999329447746
Class: rviz/Pose
Color: 0; 0; 0
Enabled: true
Head Length: 0.05000000074505806
Head Radius: 0.05000000074505806
Name: SubGoalPose
Queue Size: 10
Shaft Length: 0.029999999329447746
Shaft Radius: 0.029999999329447746
Shape: Arrow
Topic: /amr_node/sub_goal
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 0.30000001192092896
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 252; 233; 79
Enabled: false
Head Length: 0.05000000074505806
Head Radius: 0.05000000074505806
Name: ClosetPose
Queue Size: 10
Shaft Length: 0.029999999329447746
Shaft Radius: 0.029999999329447746
Shape: Arrow
Topic: /amr_node/closet_robot_goal
Unreliable: false
Value: false
- Alpha: 1
Axes Length: 0.05000000074505806
Axes Radius: 0.019999999552965164
Class: rviz/Pose
Color: 173; 127; 168
Enabled: false
Head Length: 0.05000000074505806
Head Radius: 0.05000000074505806
Name: Look A Head Pose
Queue Size: 10
Shaft Length: 0.05000000074505806
Shaft Radius: 0.029999999329447746
Shape: Axes
Topic: /amr_node/lookahead_point
Unreliable: false
Value: false
- Alpha: 1
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.029999999329447746
Class: rviz/PoseArray
Color: 0; 0; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.05000000074505806
Name: reached intermediated goals
Queue Size: 10
Shaft Length: 0.23000000417232513
Shaft Radius: 0.029999999329447746
Shape: Axes
Topic: /amr_node/reached_intermediate_goals
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 245; 121; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.019999999552965164
Length: 0.029999999329447746
Line Style: Billboards
Line Width: 0.009999999776482582
Name: transform plan
Offset:
X: 0
Y: 0
Z: 0.5
Pose Color: 138; 226; 52
Pose Style: Axes
Queue Size: 10
Radius: 0.019999999552965164
Shaft Diameter: 0.009999999776482582
Shaft Length: 0.009999999776482582
Topic: /amr_node/transformed_plan
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: false
Marker Topic: /amr_node/PredictiveTrajectory/cost_left_goals
Name: L
Namespaces:
{}
Queue Size: 100
Value: false
- Class: rviz/Marker
Enabled: false
Marker Topic: /amr_node/PredictiveTrajectory/cost_right_goals
Name: R
Namespaces:
{}
Queue Size: 100
Value: false
Enabled: true
Name: Trajectory
Enabled: true
Name: Local Map
- Alpha: 1
Axes Length: 0.05000000074505806
Axes Radius: 0.014999999664723873
Class: rviz/Pose
Color: 46; 52; 54
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Axes
Topic: /amr_node/current_goal
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/MoveCamera
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/Select
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: move_base_simple/goal
- Class: rviz/Measure
- Class: rviz/PublishPoint
Single click: true
Topic: clicked_point
Value: true
Views:
Current:
Angle: -3.135005474090576
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: -359.3964538574219
Target Frame: base_link
X: -0.49439820647239685
Y: 0.196189746260643
Saved:
- Angle: -34.55989074707031
Class: rviz/TopDownOrtho
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Invert Z Axis: false
Name: TopDownOrtho
Near Clip Distance: 0.009999999776482582
Scale: -132.97349548339844
Target Frame: base_link
X: 34.338645935058594
Y: 35.28913879394531
Window Geometry:
Displays:
collapsed: false
Height: 573
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1048
X: 0
Y: 21

11
Controllers/Packages/amr_startup/sdf/maze/model.config Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0" ?>
<model>
<name>maze</name>
<version>1.0</version>
<sdf version="1.6">model.sdf</sdf>
<author>
<name>Martin Günther</name>
<email>martin.guenther@dfki.de</email>
</author>
<description></description>
</model>

345
Controllers/Packages/amr_startup/sdf/maze/model.sdf Normal file
View File

@ -0,0 +1,345 @@
<?xml version='1.0'?>
<sdf version='1.6'>
<model name='maze'>
<pose frame=''>-0.078283 0.098984 0 0 -0 0</pose>
<link name='Wall_0'>
<collision name='Wall_0_Collision'>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_0_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0.030536 9.925 0 0 -0 0</pose>
</link>
<link name='Wall_1'>
<collision name='Wall_1_Collision'>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_1_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>9.95554 0 0 0 0 -1.5708</pose>
</link>
<link name='Wall_2'>
<collision name='Wall_2_Collision'>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_2_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>0.030536 -9.925 0 0 -0 3.14159</pose>
</link>
<link name='Wall_24'>
<collision name='Wall_24_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_24_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>5.35089 3.21906 0 0 -0 3.14159</pose>
</link>
<link name='Wall_25'>
<collision name='Wall_25_Collision'>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_25_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.25 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>4.67589 5.76906 0 0 -0 1.5708</pose>
</link>
<link name='Wall_27'>
<collision name='Wall_27_Collision'>
<geometry>
<box>
<size>5.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_27_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>7.10914 4.73454 0 0 0 -1.5708</pose>
</link>
<link name='Wall_28'>
<collision name='Wall_28_Collision'>
<geometry>
<box>
<size>3 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_28_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>3 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>8.53414 2.05954 0 0 -0 0</pose>
</link>
<link name='Wall_3'>
<collision name='Wall_3_Collision'>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_3_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>20 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-9.89446 0 0 0 -0 1.5708</pose>
</link>
<link name='Wall_30'>
<collision name='Wall_30_Collision'>
<geometry>
<box>
<size>5.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_30_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-4.35914 -2.82889 0 0 0 -1.5708</pose>
</link>
<link name='Wall_31'>
<collision name='Wall_31_Collision'>
<geometry>
<box>
<size>5.75 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_31_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>5.75 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-7.15914 -5.50389 0 0 -0 3.14159</pose>
</link>
<link name='Wall_5'>
<collision name='Wall_5_Collision'>
<geometry>
<box>
<size>16 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_5_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>16 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>-1.89911 1.86906 0 0 -0 0</pose>
</link>
<link name='Wall_6'>
<collision name='Wall_6_Collision'>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_6_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>1.5 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>6.02589 2.54406 0 0 -0 1.5708</pose>
</link>
<link name='Wall_8'>
<collision name='Wall_8_Collision'>
<geometry>
<box>
<size>0.15 0.15 2.5</size>
</box>
</geometry>
<pose frame=''>0 0 1.25 0 -0 0</pose>
</collision>
<visual name='Wall_8_Visual'>
<pose frame=''>0 0 1.25 0 -0 0</pose>
<geometry>
<box>
<size>0.15 0.15 2.5</size>
</box>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
<ambient>1 1 1 1</ambient>
</material>
</visual>
<pose frame=''>6.02589 3.21906 0 0 -0 0</pose>
</link>
<static>1</static>
</model>
</sdf>

2
pnkx_nav_core

@ -1 +1 @@
Subproject commit 11b7c4a20de070382728f8151b93528cc903d1c4 Subproject commit ca9e100bd91c94975480244be32e6ee0a3fd0f92