diff --git a/.gitmodules b/.gitmodules index f8c3a2f..4b8f1cd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,3 +5,7 @@ [submodule "pnkx_nav_core"] path = pnkx_nav_core url = https://git.pnkr.asia/HiepLM/pnkx_nav_core.git + branch = 3.0 +[submodule "pnkx_nav"] + path = pnkx_nav + url = http://git.pnkx/HiepLM/pnkx_nav.git diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 120000 index 0000000..2016816 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt index 16095a1..bb9c001 100644 --- a/Controllers/Packages/amr_control/CMakeLists.txt +++ b/Controllers/Packages/amr_control/CMakeLists.txt @@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS delta_modbus nova5_control loc_core - move_base_core amr_comunication vda5050_msgs ) @@ -138,20 +137,27 @@ add_library(${PROJECT_NAME} src/amr_monitor.cpp src/amr_safety.cpp src/amr_opc_ua_server_api.cpp - src/amr_vda_5050_client_api.cpp - src/amr_make_plan_with_order.cpp + # src/amr_vda_5050_client_api.cpp + # src/amr_make_plan_with_order.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}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(${PROJECT_NAME} PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) ## 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_control_node.cpp) -add_executable(vda_5050_api_test test/vda_5050_api.cpp) +# add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp) +# add_executable(vda_5050_api_test test/vda_5050_api.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -161,17 +167,31 @@ add_executable(vda_5050_api_test test/vda_5050_api.cpp) ## Add cmake target dependencies of the executable ## same as for the library above -add_dependencies(${PROJECT_NAME}_node - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) +# add_dependencies(${PROJECT_NAME}_node +# ${PROJECT_NAME} +# ${${PROJECT_NAME}_EXPORTED_TARGETS} +# ${catkin_EXPORTED_TARGETS} +# ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +# set_target_properties(${PROJECT_NAME}_node PROPERTIES +# SKIP_BUILD_RPATH TRUE +# BUILD_WITH_INSTALL_RPATH FALSE +# INSTALL_RPATH_USE_LINK_PATH FALSE +# ) -add_dependencies(vda_5050_api_test - ${PROJECT_NAME} - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) +# add_dependencies(vda_5050_api_test +# ${PROJECT_NAME} +# ${${PROJECT_NAME}_EXPORTED_TARGETS} +# ${catkin_EXPORTED_TARGETS} +# ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +# set_target_properties(vda_5050_api_test PROPERTIES +# SKIP_BUILD_RPATH TRUE +# BUILD_WITH_INSTALL_RPATH FALSE +# INSTALL_RPATH_USE_LINK_PATH FALSE +# ) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} @@ -180,17 +200,17 @@ target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} ) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +# target_link_libraries(${PROJECT_NAME}_node +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# ) -target_link_libraries(vda_5050_api_test - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) +# target_link_libraries(vda_5050_api_test +# ${PROJECT_NAME} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# ) ############# @@ -209,9 +229,9 @@ target_link_libraries(vda_5050_api_test ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +# install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_control.h b/Controllers/Packages/amr_control/include/amr_control/amr_control.h index ecdc99d..55d97b4 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_control.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h @@ -18,11 +18,17 @@ #include "amr_control/amr_vda_5050_client_api.h" // Objects -#include "move_base_core/navigation.h" -#include "loc_core/localization.h" -#include "amr_control/amr_monitor.h" -#include "nova5_control/imr_nova_control.h" -#include "amr_control/amr_safety.h" +#include // For TFListenerPtr +#include +#include +#include +#include +#include + +#include +// Plugin Loader +#include +#include namespace amr_control { @@ -68,6 +74,8 @@ namespace amr_control bool initalized_; ros::NodeHandle nh_; std::shared_ptr tf_; + // robot::TFListenerPtr tf_core_ptr_; + ros::Subscriber is_detected_maker_sub_; std::shared_ptr opc_ua_server_api_ptr_; @@ -77,8 +85,9 @@ namespace amr_control std::shared_ptr monitor_ptr_; std::shared_ptr monitor_thr_; - std::shared_ptr move_base_ptr_; - pluginlib::ClassLoader move_base_loader_; + move_base_core::BaseNavigation::Ptr move_base_ptr_; + boost::function move_base_loader_; + // Synchronous processing thread std::shared_ptr move_base_thr_; std::mutex init_move_base_mutex_; diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h b/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h index fef14ff..96af282 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h @@ -2,11 +2,11 @@ #define __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_ #include -#include "move_base_core/navigation.h" -#include "loc_core/localization.h" -#include "amr_comunication/opc_ua/ua_server.h" -#include "amr_control/amr_monitor.h" -#include "amr_control/common.h" +#include +#include +#include +#include +#include #include namespace amr_control diff --git a/Controllers/Packages/amr_control/package.xml b/Controllers/Packages/amr_control/package.xml index 3be4192..ed2d7a2 100644 --- a/Controllers/Packages/amr_control/package.xml +++ b/Controllers/Packages/amr_control/package.xml @@ -50,7 +50,6 @@ catkin geometry_msgs - move_base_core nav_2d_utils roscpp rospy @@ -63,7 +62,6 @@ vda5050_msgs geometry_msgs - move_base_core nav_2d_utils roscpp rospy diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp index 7d813e5..5b24768 100644 --- a/Controllers/Packages/amr_control/src/amr_control.cpp +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -13,7 +13,6 @@ namespace amr_control loc_base_ptr_(nullptr), move_base_ptr_(nullptr), amr_safety_ptr_(nullptr), - move_base_loader_("move_base_core", "move_base_core::BaseNavigation"), loc_base_loader_("loc_core", "loc_core::BaseLocalization") { this->init(nh, buffer); @@ -29,7 +28,6 @@ namespace amr_control loc_base_ptr_(nullptr), move_base_ptr_(nullptr), amr_safety_ptr_(nullptr), - move_base_loader_("move_base_core", "move_base_core::BaseNavigation"), loc_base_loader_("loc_core", "loc_core::BaseLocalization") { } @@ -221,358 +219,366 @@ namespace amr_control throw std::runtime_error("tf2_ros::Buffer object is null"); try { - move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name); - ROS_INFO("Created object %s susseced", obj_name.c_str()); - move_base_ptr_->initialize(tf_); - ros::Rate r(3); - do - { - r.sleep(); - ros::spinOnce(); - } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready); + // robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__); + // robot::PluginLoaderHelper loader; + // std::string path_file_so = loader.findLibraryPath("MoveBase"); + // move_base_loader_ = boost::dll::import_alias( + // path_file_so, + // "MoveBase", boost::dll::load_mode::append_decorations); + + // robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__); + // move_base_ptr_ = create_plugin(); + // move_base_ptr_->initialize(tf_core_ptr_); - if (move_base_ptr_ != nullptr && - move_base_ptr_->nav_feedback_ != nullptr && - move_base_ptr_->nav_feedback_->is_ready) - { - geometry_msgs::Vector3 linear; - linear.x = 0.3; - move_base_ptr_->setTwistLinear(linear); - linear.x = -0.3; - move_base_ptr_->setTwistLinear(linear); - } + // ros::Rate r(3); + // do + // { + // r.sleep(); + // ros::spinOnce(); + // } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready); + + // if (move_base_ptr_ != nullptr && + // move_base_ptr_->nav_feedback_ != nullptr && + // move_base_ptr_->nav_feedback_->is_ready) + // { + // geometry_msgs::Vector3 linear; + // linear.x = 0.3; + // move_base_ptr_->setTwistLinear(linear); + // linear.x = -0.3; + // move_base_ptr_->setTwistLinear(linear); + // } } - catch (pluginlib::PluginlibException &ex) + catch (const std::exception &e) { - ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what()); + // robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what()); exit(1); } - catch (std::exception &e) + catch (...) { - ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what()); + // robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__); exit(1); } } - void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh) - { - this->monitor_ptr_ = std::make_shared(nh); - } + // void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh) + // { + // this->monitor_ptr_ = std::make_shared(nh); + // } - void AmrController::ArmCallBack() - { - ROS_INFO("Arm Calling"); - std::lock_guard lock(this->arm_mutex_); - { - arm_joined_ = true; - this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this); - } - } + // void AmrController::ArmCallBack() + // { + // ROS_INFO("Arm Calling"); + // std::lock_guard lock(this->arm_mutex_); + // { + // arm_joined_ = true; + // this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this); + // } + // } - void AmrController::ArmDotuff() - { - std::shared_ptr arm_control_ptr; - arm_control_ptr = std::make_shared(); - arm_control_ptr->enable_ = &this->enable_; - arm_control_ptr->go_home_flag_ = &this->arm_go_home_; - arm_control_ptr->continue_flag_ = &this->arm_continue_; - arm_control_ptr->power_on_flag_ = &this->arm_power_on_; - OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_; - this->status_code_ptr_ = reinterpret_cast(&arm_control_ptr->statusCode_); + // void AmrController::ArmDotuff() + // { + // std::shared_ptr arm_control_ptr; + // arm_control_ptr = std::make_shared(); + // arm_control_ptr->enable_ = &this->enable_; + // arm_control_ptr->go_home_flag_ = &this->arm_go_home_; + // arm_control_ptr->continue_flag_ = &this->arm_continue_; + // arm_control_ptr->power_on_flag_ = &this->arm_power_on_; + // OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_; + // this->status_code_ptr_ = reinterpret_cast(&arm_control_ptr->statusCode_); - if (!this->arm_go_home_) - { - arm_control_ptr->ok_count_max_ = &this->count_ok_max_; - arm_control_ptr->ng_count_max_ = &this->count_ng_max_; - arm_control_ptr->startModeThread(); - } - else - { - arm_control_ptr->startHomeThread(); - } + // if (!this->arm_go_home_) + // { + // arm_control_ptr->ok_count_max_ = &this->count_ok_max_; + // arm_control_ptr->ng_count_max_ = &this->count_ng_max_; + // arm_control_ptr->startModeThread(); + // } + // else + // { + // arm_control_ptr->startHomeThread(); + // } - arm_control_ptr.reset(); - ROS_INFO("Arm Finished"); - std::lock_guard lock(this->arm_mutex_); - this->arm_joined_ = false; - } + // arm_control_ptr.reset(); + // ROS_INFO("Arm Finished"); + // std::lock_guard lock(this->arm_mutex_); + // this->arm_joined_ = false; + // } - void AmrController::unLoadCallBack() - { - std::lock_guard lock(this->arm_mutex_); - { - ROS_INFO("Shiping call"); - this->belt_joined_ = true; - this->cancel_ = false; - this->cur_belt_state_en_ = amr_control::State::WAITING; - this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_)); - } - } + // void AmrController::unLoadCallBack() + // { + // std::lock_guard lock(this->arm_mutex_); + // { + // ROS_INFO("Shiping call"); + // this->belt_joined_ = true; + // this->cancel_ = false; + // this->cur_belt_state_en_ = amr_control::State::WAITING; + // this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_)); + // } + // } - void AmrController::conveyorBeltsShipping(amr_control::State &state) - { - state = amr_control::State::INITIALIZING; - std::shared_ptr plc_controller_ptr_; - plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); - plc_controller_ptr_->connect(); + // void AmrController::conveyorBeltsShipping(amr_control::State &state) + // { + // state = amr_control::State::INITIALIZING; + // std::shared_ptr plc_controller_ptr_; + // plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + // plc_controller_ptr_->connect(); - if (!plc_controller_ptr_->checkConnected()) - { - state = amr_control::State::FAILED; - return; - } + // if (!plc_controller_ptr_->checkConnected()) + // { + // state = amr_control::State::FAILED; + // return; + // } - if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) - { - int shipping_regsister = 119, receiving_regsister = 116; - plc_controller_ptr_->setM(shipping_regsister); - plc_controller_ptr_->resetM(receiving_regsister); - ros::Duration(0.1).sleep(); - plc_controller_ptr_->resetM(shipping_regsister); - plc_controller_ptr_->resetM(receiving_regsister); + // if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + // { + // int shipping_regsister = 119, receiving_regsister = 116; + // plc_controller_ptr_->setM(shipping_regsister); + // plc_controller_ptr_->resetM(receiving_regsister); + // ros::Duration(0.1).sleep(); + // plc_controller_ptr_->resetM(shipping_regsister); + // plc_controller_ptr_->resetM(receiving_regsister); - ros::Rate r(5); - while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) - { - state = amr_control::State::RUNNING; - bool output_belt[2]; - plc_controller_ptr_->mulGetM(124, 125, output_belt); + // ros::Rate r(5); + // while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) + // { + // state = amr_control::State::RUNNING; + // bool output_belt[2]; + // plc_controller_ptr_->mulGetM(124, 125, output_belt); - bool enable_shipping = output_belt[0]; - bool enable_receiving = output_belt[1]; - if (!enable_shipping && !enable_receiving) - { - state = amr_control::State::FINISHED; - break; - } - r.sleep(); - } + // bool enable_shipping = output_belt[0]; + // bool enable_receiving = output_belt[1]; + // if (!enable_shipping && !enable_receiving) + // { + // state = amr_control::State::FINISHED; + // break; + // } + // r.sleep(); + // } - if (cancel_ || !plc_controller_ptr_->checkConnected()) - state = amr_control::State::FAILED; + // if (cancel_ || !plc_controller_ptr_->checkConnected()) + // state = amr_control::State::FAILED; - plc_controller_ptr_->close(); - } - if (plc_controller_ptr_) - plc_controller_ptr_.reset(); + // plc_controller_ptr_->close(); + // } + // if (plc_controller_ptr_) + // plc_controller_ptr_.reset(); - std::lock_guard lock(this->arm_mutex_); - this->belt_joined_ = false; - } + // std::lock_guard lock(this->arm_mutex_); + // this->belt_joined_ = false; + // } - void AmrController::loadCallBack() - { - std::lock_guard lock(this->arm_mutex_); - { - ROS_INFO("Receiving call"); - this->belt_joined_ = true; - this->cancel_ = false; - this->cur_belt_state_en_ = amr_control::State::WAITING; - this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_)); - } - } + // void AmrController::loadCallBack() + // { + // std::lock_guard lock(this->arm_mutex_); + // { + // ROS_INFO("Receiving call"); + // this->belt_joined_ = true; + // this->cancel_ = false; + // this->cur_belt_state_en_ = amr_control::State::WAITING; + // this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_)); + // } + // } - void AmrController::conveyorBeltsReceiving(amr_control::State &state) - { - state = amr_control::State::INITIALIZING; - std::shared_ptr plc_controller_ptr_; - plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); - plc_controller_ptr_->connect(); + // void AmrController::conveyorBeltsReceiving(amr_control::State &state) + // { + // state = amr_control::State::INITIALIZING; + // std::shared_ptr plc_controller_ptr_; + // plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + // plc_controller_ptr_->connect(); - if (!plc_controller_ptr_->checkConnected()) - { - state = amr_control::State::FAILED; - return; - } + // if (!plc_controller_ptr_->checkConnected()) + // { + // state = amr_control::State::FAILED; + // return; + // } - if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) - { - int shipping_regsister = 119, receiving_regsister = 116; - plc_controller_ptr_->resetM(shipping_regsister); - plc_controller_ptr_->setM(receiving_regsister); - ros::Duration(0.1).sleep(); - plc_controller_ptr_->resetM(shipping_regsister); - plc_controller_ptr_->resetM(receiving_regsister); + // if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + // { + // int shipping_regsister = 119, receiving_regsister = 116; + // plc_controller_ptr_->resetM(shipping_regsister); + // plc_controller_ptr_->setM(receiving_regsister); + // ros::Duration(0.1).sleep(); + // plc_controller_ptr_->resetM(shipping_regsister); + // plc_controller_ptr_->resetM(receiving_regsister); - ros::Rate r(5); - while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) - { - state = amr_control::State::RUNNING; - bool output_belt[2]; - plc_controller_ptr_->mulGetM(124, 125, output_belt); + // ros::Rate r(5); + // while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_) + // { + // state = amr_control::State::RUNNING; + // bool output_belt[2]; + // plc_controller_ptr_->mulGetM(124, 125, output_belt); - bool enable_shipping = output_belt[0]; - bool enable_receiving = output_belt[1]; - if (!enable_shipping && !enable_receiving) - { - state = amr_control::State::FINISHED; - break; - } - r.sleep(); - } - if (this->cancel_ || !plc_controller_ptr_->checkConnected()) - state = amr_control::State::FAILED; + // bool enable_shipping = output_belt[0]; + // bool enable_receiving = output_belt[1]; + // if (!enable_shipping && !enable_receiving) + // { + // state = amr_control::State::FINISHED; + // break; + // } + // r.sleep(); + // } + // if (this->cancel_ || !plc_controller_ptr_->checkConnected()) + // state = amr_control::State::FAILED; - plc_controller_ptr_->close(); - } - if (plc_controller_ptr_) - plc_controller_ptr_.reset(); + // plc_controller_ptr_->close(); + // } + // if (plc_controller_ptr_) + // plc_controller_ptr_.reset(); - std::lock_guard lock(this->arm_mutex_); - this->belt_joined_ = false; - } + // std::lock_guard lock(this->arm_mutex_); + // this->belt_joined_ = false; + // } - void AmrController::controllerDotuff() - { - ros::Rate r(10); - while (ros::ok()) - { - std::shared_ptr plc_controller_ptr_; - plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); - plc_controller_ptr_->connect(); - r.sleep(); - ros::spinOnce(); + // void AmrController::controllerDotuff() + // { + // ros::Rate r(10); + // while (ros::ok()) + // { + // std::shared_ptr plc_controller_ptr_; + // plc_controller_ptr_ = std::make_shared("192.168.2.5", 502); + // plc_controller_ptr_->connect(); + // r.sleep(); + // ros::spinOnce(); - if (plc_controller_ptr_ == nullptr) - continue; - if (!plc_controller_ptr_->checkConnected()) - continue; + // if (plc_controller_ptr_ == nullptr) + // continue; + // if (!plc_controller_ptr_->checkConnected()) + // continue; - this->amr_safety_ptr_ = std::make_shared(); - this->amr_safety_ptr_->getController(plc_controller_ptr_); + // this->amr_safety_ptr_ = std::make_shared(); + // this->amr_safety_ptr_->getController(plc_controller_ptr_); - while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) - { - if (!this->monitor_ptr_) - continue; - nav_2d_msgs::Twist2D velocity; - if (this->monitor_ptr_->getVelocity(velocity)) - { - cmd_vel_mtx.lock(); - this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_); - cmd_vel_mtx.unlock(); - } + // while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected()) + // { + // if (!this->monitor_ptr_) + // continue; + // nav_2d_msgs::Twist2D velocity; + // if (this->monitor_ptr_->getVelocity(velocity)) + // { + // cmd_vel_mtx.lock(); + // this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_); + // cmd_vel_mtx.unlock(); + // } - if (move_base_ptr_ != nullptr && - move_base_ptr_->nav_feedback_ != nullptr && - move_base_ptr_->nav_feedback_->is_ready) - { - if (velocity.x <= -0.01) - this->amr_safety_ptr_->writeMutesSafety(true); - else - { - this->amr_safety_ptr_->writeMutesSafety(this->muted_); - } - } + // if (move_base_ptr_ != nullptr && + // move_base_ptr_->nav_feedback_ != nullptr && + // move_base_ptr_->nav_feedback_->is_ready) + // { + // if (velocity.x <= -0.01) + // this->amr_safety_ptr_->writeMutesSafety(true); + // else + // { + // this->amr_safety_ptr_->writeMutesSafety(this->muted_); + // } + // } - bool have_goods; - int have_goods_regsister = 142; - if (!OpcUAServerAPI::belt_cancel_) - { - plc_controller_ptr_->getM(have_goods_regsister, have_goods); - OpcUAServerAPI::have_goods_ = have_goods; - } - else - { - plc_controller_ptr_->resetM(have_goods_regsister); - } + // bool have_goods; + // int have_goods_regsister = 142; + // if (!OpcUAServerAPI::belt_cancel_) + // { + // plc_controller_ptr_->getM(have_goods_regsister, have_goods); + // OpcUAServerAPI::have_goods_ = have_goods; + // } + // else + // { + // plc_controller_ptr_->resetM(have_goods_regsister); + // } - if (vda_5050_client_api_ptr_) - { - amr_control::OperatingMode mode_; - bool operating_mode[4]; - plc_controller_ptr_->mulGetM(14, 17, operating_mode); - if (operating_mode[0]) - mode_ = amr_control::OperatingMode::AUTOMATIC; - else if (operating_mode[2]) - mode_ = amr_control::OperatingMode::SERVICE; - else if (operating_mode[3]) - mode_ = amr_control::OperatingMode::MANUAL; + // if (vda_5050_client_api_ptr_) + // { + // amr_control::OperatingMode mode_; + // bool operating_mode[4]; + // plc_controller_ptr_->mulGetM(14, 17, operating_mode); + // if (operating_mode[0]) + // mode_ = amr_control::OperatingMode::AUTOMATIC; + // else if (operating_mode[2]) + // mode_ = amr_control::OperatingMode::SERVICE; + // else if (operating_mode[3]) + // mode_ = amr_control::OperatingMode::MANUAL; - switch (mode_) - { - case amr_control::OperatingMode::AUTOMATIC: - vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC; - break; - case amr_control::OperatingMode::MANUAL: - vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL; - break; - case amr_control::OperatingMode::SERVICE: - vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; - break; - default: - vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default - break; - } - } - r.sleep(); - ros::spinOnce(); - } - if (plc_controller_ptr_) - plc_controller_ptr_.reset(); - if (amr_safety_ptr_) - amr_safety_ptr_.reset(); - } - } + // switch (mode_) + // { + // case amr_control::OperatingMode::AUTOMATIC: + // vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC; + // break; + // case amr_control::OperatingMode::MANUAL: + // vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL; + // break; + // case amr_control::OperatingMode::SERVICE: + // vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; + // break; + // default: + // vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default + // break; + // } + // } + // r.sleep(); + // ros::spinOnce(); + // } + // if (plc_controller_ptr_) + // plc_controller_ptr_.reset(); + // if (amr_safety_ptr_) + // amr_safety_ptr_.reset(); + // } + // } - void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg) - { - this->muted_ = msg->data; - } + // void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg) + // { + // this->muted_ = msg->data; + // } - void AmrController::threadHandle() - { - ros::Rate r(5); - while (ros::ok()) - { - if (this->arm_thread_.joinable()) - { - std::lock_guard lock(this->arm_mutex_); - { - if (!this->arm_joined_) - { - this->arm_thread_.join(); - } - } - } + // void AmrController::threadHandle() + // { + // ros::Rate r(5); + // while (ros::ok()) + // { + // if (this->arm_thread_.joinable()) + // { + // std::lock_guard lock(this->arm_mutex_); + // { + // if (!this->arm_joined_) + // { + // this->arm_thread_.join(); + // } + // } + // } - if (this->belt_thread_.joinable()) - { - std::lock_guard lock(this->belt_mutex_); - { - if (!this->belt_joined_) - { - this->belt_thread_.join(); - } - } - } + // if (this->belt_thread_.joinable()) + // { + // std::lock_guard lock(this->belt_mutex_); + // { + // if (!this->belt_joined_) + // { + // this->belt_thread_.join(); + // } + // } + // } - if (move_base_ptr_ != nullptr && - move_base_ptr_->nav_feedback_ != nullptr && - move_base_ptr_->nav_feedback_->is_ready) - { - nav_2d_msgs::Twist2D 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_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta); + // if (move_base_ptr_ != nullptr && + // move_base_ptr_->nav_feedback_ != nullptr && + // move_base_ptr_->nav_feedback_->is_ready) + // { + // nav_2d_msgs::Twist2D 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_.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; - geometry_msgs::Vector3 angular; + // geometry_msgs::Vector3 linear; + // geometry_msgs::Vector3 angular; - cmd_vel_mtx.lock(); - linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x; - angular.z = this->cmd_vel_max_.theta; - cmd_vel_mtx.unlock(); + // cmd_vel_mtx.lock(); + // linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x; + // angular.z = this->cmd_vel_max_.theta; + // cmd_vel_mtx.unlock(); - this->move_base_ptr_->setTwistLinear(linear); - linear.x *= -1.0; - this->move_base_ptr_->setTwistLinear(linear); - this->move_base_ptr_->setTwistAngular(angular); - } - } - r.sleep(); - ros::spinOnce(); - } - } + // this->move_base_ptr_->setTwistLinear(linear); + // linear.x *= -1.0; + // this->move_base_ptr_->setTwistLinear(linear); + // this->move_base_ptr_->setTwistAngular(angular); + // } + // } + // r.sleep(); + // ros::spinOnce(); + // } + // } } \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp index 4eaaf54..8aeed14 100644 --- a/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp +++ b/Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp @@ -155,3298 +155,3298 @@ void amr_control::OpcUAServerAPI::defineObjects() amr_control::OpcUAServerAPI::addConveyorBeltState(this->server_ptr_->getServerObject(), conveyor_belt_Id); } -void amr_control::OpcUAServerAPI::addStartMappingMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Bắt đầu quá trình tạo bản đồ "); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartMapping"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"StartMapping"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"StartMapping"), - incAttr, &amr_control::OpcUAServerAPI::startMappingCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::startMappingCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu tạo bản đồ gọi thành công"); - - std::thread([=]() - { - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - amr_control::OpcUAServerAPI::loc_base_ptr_->startMapping(); }) - .detach(); - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addStopMappingMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Kết thúc quá trình tạo bản đồ "); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopMapping"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"StopMapping"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"StopMapping"), - incAttr, &amr_control::OpcUAServerAPI::stopMappingCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::stopMappingCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); - - std::thread([=]() - { - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - amr_control::OpcUAServerAPI::loc_base_ptr_->stopMapping(); }) - .detach(); - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Bắt đầu quá trình định vị bản đồ "); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartLocalization"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"StartLocalization"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"StartLocalization"), - incAttr, &amr_control::OpcUAServerAPI::startLocalizationCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::startLocalizationCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ gọi thành công"); - - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - amr_control::OpcUAServerAPI::loc_base_ptr_->startLocalization(); - else - { - message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ bị lỗi"); - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Kết thúc quá trình định vị bản đồ "); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopLocalization"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"StopLocalization"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"StopLocalization"), - incAttr, &amr_control::OpcUAServerAPI::stopLocalizationCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::stopLocalizationCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); - - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - amr_control::OpcUAServerAPI::loc_base_ptr_->stopLocalization(); - else - { - message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addListMapFilesMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[1]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"List files"); - outputArguments[0].name = UA_STRING((char *)"List files"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Liệt kê các file bản đồ."); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GetMapFiles"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"GetMapFiles"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"GetMapFiles"), - incAttr, &amr_control::OpcUAServerAPI::listMapFilesCallBack, - 0, NULL, 1, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::listMapFilesCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Variant_init(output); - UA_String message = UA_STRING_ALLOC((char *)""); - std::stringstream result; - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - { - amr_control::OpcUAServerAPI::loc_base_ptr_->listMapFiles(result); - ROS_INFO_STREAM(result.str()); - message = UA_STRING_ALLOC((char *)result.str().c_str()); - } - - UA_Variant_setScalarCopy(output, &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Hoàn thành"); - return UA_STATUSCODE_GOOD; -} - -void amr_control::OpcUAServerAPI::addActivateMapMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One input argument */ - UA_Argument inputArguments[1]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MapName"); - inputArguments[0].name = UA_STRING((char *)"MapName"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chọn một map file để định vị bản đồ "); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ActivateMap"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"ActivateMap"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"ActivateMap"), - incAttr, &amr_control::OpcUAServerAPI::activateMapCallBack, - 1, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::activateMapCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_String map_name = *(UA_String *)input[0].data; - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ gọi thành công"); - bool result; - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - { - std::string map_name_str = std::string(reinterpret_cast(map_name.data), map_name.length); - if (!map_name_str.empty()) - result = amr_control::OpcUAServerAPI::loc_base_ptr_->changeStaticMap(map_name_str); - else - message = UA_STRING_ALLOC((char *)"MapName chưa có thông tin"); - } - - if (!result) - { - message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ bị lỗi"); - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[3]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); - inputArguments[0].name = UA_STRING((char *)"X"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); - inputArguments[1].name = UA_STRING((char *)"Y"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[2]); - inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"YAW"); - inputArguments[2].name = UA_STRING((char *)"YAW"); - inputArguments[2].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[2].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Set vị trí ban đầu của robot trên bản đồ."); - - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Initial Pose"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Initial Pose"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Initial Pose"), - incAttr, &amr_control::OpcUAServerAPI::setInitialPoseCallBack, - 3, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::setInitialPoseCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Double X = *(UA_Double *)input[0].data; - UA_Double Y = *(UA_Double *)input[1].data; - UA_Double Yaw = *(UA_Double *)input[2].data; - - std::string fixed_frame = "map"; - geometry_msgs::PoseWithCovarianceStamped pose; - pose.header.frame_id = fixed_frame; - pose.header.stamp = ros::Time::now(); - - // set x,y coord - pose.pose.pose.position.x = X; - pose.pose.pose.position.y = Y; - pose.pose.pose.position.z = 0.0; - - // set theta - tf::Quaternion quat; - quat.setRPY(0.0, 0.0, Yaw); - tf::quaternionTFToMsg(quat, pose.pose.pose.orientation); - - pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5; - pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5; - pose.pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0; - - amr_control::OpcUAServerAPI::init_pub_.publish(pose); - - UA_Variant_init(output); - - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Initial Pose gọi thành công"); - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose bị lỗi"); - return UA_STATUSCODE_BAD; -} - -UA_NodeId *amr_control::OpcUAServerAPI::robotPoseX_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::robotPoseY_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::robotPoseYaw_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::currentActiveMap_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::slamState_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::workingDirectory_Id_ = new UA_NodeId(); -void amr_control::OpcUAServerAPI::addRobotPoseProperty(UA_Server *server, UA_NodeId parentID) -{ - UA_NodeId robotPose_Id; /* get the nodeid assigned by the server */ - UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default; - slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RobotPose"); - UA_Server_addObjectNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), - UA_QUALIFIEDNAME(1, (char *)"RobotPose"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), - slamAttr, NULL, &robotPose_Id); - - // UA_NodeId robotPoseX_Id_; - UA_VariableAttributes poseXAttr = UA_VariableAttributes_default; - UA_Double pose_x = 0; - UA_Variant_setScalar(&poseXAttr.value, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); - poseXAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); - poseXAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ X của robot."); - poseXAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"X"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - poseXAttr, NULL, amr_control::OpcUAServerAPI::robotPoseX_Id_); - - // UA_NodeId robotPoseY_Id_; - UA_VariableAttributes poseYAttr = UA_VariableAttributes_default; - UA_Double pose_y = 0; - UA_Variant_setScalar(&poseYAttr.value, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); - poseYAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); - poseYAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Y của robot."); - poseYAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Y"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - poseYAttr, NULL, amr_control::OpcUAServerAPI::robotPoseY_Id_); - - // UA_NodeId robotPoseYaw_Id_; - UA_VariableAttributes poseYawAttr = UA_VariableAttributes_default; - UA_Double pose_yaw = 0; - UA_Variant_setScalar(&poseYawAttr.value, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); - poseYawAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Yaw"); - poseYawAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Yaw của robot."); - poseYawAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Yaw"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - poseYawAttr, NULL, amr_control::OpcUAServerAPI::robotPoseYaw_Id_); - - // UA_NodeId currentActiveMap_Id; - UA_VariableAttributes currentActiveMapAttr = UA_VariableAttributes_default; - UA_String current_active_map = UA_STRING_ALLOC((char *)" "); - UA_Variant_setScalar(¤tActiveMapAttr.value, ¤t_active_map, &UA_TYPES[UA_TYPES_STRING]); - currentActiveMapAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CurrentActiveMap"); - currentActiveMapAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến lưu trữ thông tin về map đang được active."); - currentActiveMapAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - currentActiveMapAttr, NULL, amr_control::OpcUAServerAPI::currentActiveMap_Id_); - - // UA_NodeId slamState_Id_; - UA_VariableAttributes slamStateAttr = UA_VariableAttributes_default; - UA_String slam_state = UA_STRING_ALLOC((char *)" "); - UA_Variant_setScalar(&slamStateAttr.value, &slam_state, &UA_TYPES[UA_TYPES_STRING]); - slamStateAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SlamState"); - slamStateAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến trạng thái của module slam: Mapping, Localization, Calibrations, Ready, Error"); - slamStateAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - slamStateAttr, NULL, amr_control::OpcUAServerAPI::slamState_Id_); - - // UA_NodeId workingDirectory_Id_ - UA_VariableAttributes workingDirectoryAttr = UA_VariableAttributes_default; - UA_String working_directory = UA_STRING_ALLOC((char *)" "); - UA_Variant_setScalar(&workingDirectoryAttr.value, &working_directory, &UA_TYPES[UA_TYPES_STRING]); - workingDirectoryAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"WorkingDirectories"); - workingDirectoryAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Đường dẫn lưu trữ các file bản đồ"); - workingDirectoryAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"WorkingDirectories"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - workingDirectoryAttr, NULL, amr_control::OpcUAServerAPI::workingDirectory_Id_); -} - -void amr_control::OpcUAServerAPI::slamHandle() -{ - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) - { - geometry_msgs::Pose2D robot_pose; - if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(robot_pose)) - { - UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "getRobotPose: Is Not OKAY"); - } - else - { - UA_Double pose_x = std::round(robot_pose.x * 10000.0) / 10000.0; - UA_Double pose_y = std::round(robot_pose.y * 10000.0) / 10000.0; - UA_Double pose_yaw = std::round(robot_pose.theta * 10000.0) / 10000.0; - - if (amr_control::OpcUAServerAPI::robotPoseX_Id_ != NULL && - amr_control::OpcUAServerAPI::robotPoseY_Id_ != NULL && - amr_control::OpcUAServerAPI::robotPoseYaw_Id_ != NULL && - amr_control::OpcUAServerAPI::currentActiveMap_Id_ != NULL && - amr_control::OpcUAServerAPI::slamState_Id_ != NULL && - amr_control::OpcUAServerAPI::workingDirectory_Id_ != NULL) - { - // Cập nhật giá trị X - UA_Variant poseXVariant; - UA_Variant_setScalar(&poseXVariant, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseX_Id_, poseXVariant); // Truyền đối tượng, không phải con trỏ - - // Cập nhật giá trị Y - UA_Variant poseYVariant; - UA_Variant_setScalar(&poseYVariant, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseY_Id_, poseYVariant); // Truyền đối tượng, không phải con trỏ - - // Cập nhật giá trị Yaw - UA_Variant poseYawVariant; - UA_Variant_setScalar(&poseYawVariant, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseYaw_Id_, poseYawVariant); - - if (amr_control::OpcUAServerAPI::loc_base_ptr_) - { - // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); - // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); - UA_Variant currentActiveMapVariant; - UA_String activated_map_filename = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); - UA_Variant_setScalar(¤tActiveMapVariant, &activated_map_filename, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::currentActiveMap_Id_, currentActiveMapVariant); - - UA_Variant workingDirectoryvariant; - UA_String working_dir = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); - UA_Variant_setScalar(&workingDirectoryvariant, &working_dir, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::workingDirectory_Id_, workingDirectoryvariant); - - UA_String slam_state = UA_STRING_ALLOC((char *)" "); - switch (amr_control::OpcUAServerAPI::loc_base_ptr_->getState()) - { - case loc_core::Mapping: - slam_state = UA_STRING_ALLOC((char *)"Mapping"); - break; - case loc_core::Localization: - slam_state = UA_STRING_ALLOC((char *)"Localization"); - break; - case loc_core::Calibrations: - slam_state = UA_STRING_ALLOC((char *)"Calibrations"); - break; - case loc_core::Ready: - slam_state = UA_STRING_ALLOC((char *)"Ready"); - break; - case loc_core::Error: - slam_state = UA_STRING_ALLOC((char *)"Error"); - break; - default: - break; - } - UA_Variant slamStatevariant; - UA_Variant_setScalar(&slamStatevariant, &slam_state, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::slamState_Id_, slamStatevariant); - } - } - } - } -} - -void amr_control::OpcUAServerAPI::addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[2]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); - inputArguments[0].name = UA_STRING((char *)"Length"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); - inputArguments[1].name = UA_STRING((char *)"Nodes"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; - inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; - inputArguments[1].arrayDimensionsSize = 2; - inputArguments[1].arrayDimensions = pInputDimension; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message với độ chính xác mong muốn."); - - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToNode"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"MoveToNode"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"MoveToNode"), - incAttr, &amr_control::OpcUAServerAPI::moveToNodeCallBack, - 2, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::moveToNodeCallBack( - UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_UInt32 length = *(UA_UInt32 *)input[0].data; - UA_Double *nodes = (UA_Double *)input[1].data; - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"MoveToNode gọi thành công"); - - if (input[1].arrayDimensionsSize != 2) - return UA_STATUSCODE_BADINTERNALERROR; - - const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; - const int row = input[1].arrayDimensions[1]; - - if (length > column) - { - message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); - goto STATUSCODE_BADREQUESTTOOLARGE; - } - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - UA_Double matrix[column][row]; - for (int i = 0; i < column; i++) - for (int j = 0; j < row; j++) - matrix[i][j] = nodes[i * row + j]; - - geometry_msgs::Pose2D goal_2d; - goal_2d.x = matrix[column - 1][0]; - goal_2d.y = matrix[column - 1][1]; - goal_2d.theta = matrix[column - 1][2]; - const double velocity = matrix[column - 1][3]; - const double xy_tolerance = matrix[column - 1][4]; - const double yaw_tolerance = 0.025; - const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); - amr_control::OpcUAServerAPI::move_base_ptr_->moveTo(goal, xy_tolerance, yaw_tolerance); - amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; - amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; - geometry_msgs::Vector3 linear; - linear.x = velocity; - amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addDockToNodeMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[3]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); - inputArguments[0].name = UA_STRING((char *)"Length"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); - inputArguments[1].name = UA_STRING((char *)"Nodes"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; - inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; - inputArguments[1].arrayDimensionsSize = 2; - inputArguments[1].arrayDimensions = pInputDimension; - - UA_Argument_init(&inputArguments[2]); - inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Maker"); - inputArguments[2].name = UA_STRING((char *)"Maker"); - inputArguments[2].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - inputArguments[2].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là lấy hàng ở một vị trí lận cận với điểm đích được order."); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToPickup"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"MoveToPickup"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"MoveToPickup"), - incAttr, &amr_control::OpcUAServerAPI::dockToNodeCallBack, - 2, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::dockToNodeCallBack( - UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_UInt32 length = *(UA_UInt32 *)input[0].data; - UA_Double *nodes = (UA_Double *)input[1].data; - UA_String maker = *(UA_String *)input[2].data; - - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"MoveToPickUp gọi thành công"); - - if (input[1].arrayDimensionsSize != 2) - return UA_STATUSCODE_BADINTERNALERROR; - - const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; - const int row = input[1].arrayDimensions[1]; - - if (length > column) - { - message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); - goto STATUSCODE_BADREQUESTTOOLARGE; - } - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - UA_Double matrix[column][row]; - for (int i = 0; i < column; i++) - for (int j = 0; j < row; j++) - matrix[i][j] = nodes[i * row + j]; - - geometry_msgs::Pose2D goal_2d; - goal_2d.x = matrix[column - 1][0]; - goal_2d.y = matrix[column - 1][1]; - goal_2d.theta = matrix[column - 1][2]; - const double velocity = matrix[column - 1][3]; - const double xy_tolerance = matrix[column - 1][4]; - const double yaw_tolerance = 0.025; - const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); - std::string maker_str(reinterpret_cast(maker.data), maker.length); - amr_control::OpcUAServerAPI::move_base_ptr_->dockTo(maker_str, goal, xy_tolerance, yaw_tolerance); - amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; - amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; - geometry_msgs::Vector3 linear; - linear.x = velocity; - amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToPickUp gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addRotateToMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[2]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Góc muốn quay trong giải từ -Pi đến Pi"); - inputArguments[0].name = UA_STRING((char *)"Yaw goal"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Sai số góc cho phép"); - inputArguments[1].name = UA_STRING((char *)"Yaw tolerance"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_INT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Robot quay tại chỗ một góc yaw."); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RotateTo"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"RotateTo"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"RotateTo"), - incAttr, &amr_control::OpcUAServerAPI::rotateToCallBack, - 2, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::rotateToCallBack( - UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Double Yaw = *(UA_Double *)input[0].data; - UA_Double tolerance = *(UA_Double *)input[1].data; - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Successed"); - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - geometry_msgs::PoseStamped goal; - if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(goal)) - { - message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); - goto STATUSCODE_BADINTERNALERROR; - } - tf2::Quaternion q; - q.setRPY(0, 0, angles::normalize_angle(Yaw)); - goal.pose.orientation.x = q.x(); - goal.pose.orientation.y = q.y(); - goal.pose.orientation.z = q.z(); - goal.pose.orientation.w = q.w(); - amr_control::OpcUAServerAPI::move_base_ptr_->rotateTo(goal, tolerance); - } - else - { - return UA_STATUSCODE_BADSTRUCTUREMISSING; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo was called"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BAD: - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addMoveStraightMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[2]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Khoảng cách đi '+' là tiến '-' là lùi"); - inputArguments[0].name = UA_STRING((char *)"Distance"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc di chuyển"); - inputArguments[1].name = UA_STRING((char *)"Velocity linear"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - inputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là trả hàng ở một vị trí lận cận với điểm đích được order."); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveStraight"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"MoveStraight"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"MoveStraight"), - incAttr, &amr_control::OpcUAServerAPI::moveStraighCallBack, - 2, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::moveStraighCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Double distance = *(UA_Double *)input[0].data; - UA_Double velocity = *(UA_Double *)input[1].data; - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"MoveStraight gọi thành công"); - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - geometry_msgs::PoseStamped pose; - if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(pose)) - { - message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); - goto STATUSCODE_BADINTERNALERROR; - } - ROS_INFO_STREAM(pose); - geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); - amr_control::OpcUAServerAPI::move_base_ptr_->moveStraightTo(goal); - amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; - amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; - geometry_msgs::Vector3 linear; - linear.x = velocity; - amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveStraight gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addCancelMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Hủy lệnh di chuyển của AMR"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Cancel"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Cancel"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Cancel"), - incAttr, &amr_control::OpcUAServerAPI::cancelCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::cancelCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Cancel gọi thành công"); - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - amr_control::OpcUAServerAPI::move_base_ptr_->cancel(); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng hủy lệnh đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addPauseMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Lệnh tạm dừng di chuyển của AMR"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Pause"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Pause"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Pause"), - incAttr, &amr_control::OpcUAServerAPI::pauseCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::pauseCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Pause gọi thành công"); - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - amr_control::OpcUAServerAPI::move_base_ptr_->pause(); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng tạm dừng đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addResumeMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Lệnh tiếp tục di chuyển của AMR"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Resume"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Resume"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Resume"), - incAttr, &amr_control::OpcUAServerAPI::resumeCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::resumeCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Resume gọi thành công"); - - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) - { - amr_control::OpcUAServerAPI::move_base_ptr_->resume(); - } - else - { - message = UA_STRING_ALLOC((char *)"Chức năng tiếp tục đang bị lỗi khởi tạo"); - goto STATUSCODE_BADSTRUCTUREMISSING; - } - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addResetMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Lệnh reset AMR"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Reset"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Reset"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Reset"), - incAttr, &amr_control::OpcUAServerAPI::resetCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::resetCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - // sleep(5); - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Reset gọi thành công"); - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addWriteMutedMethod(UA_Server *server, UA_NodeId parentID) -{ - UA_Argument inputArguments[1]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Tích là bật hoặc ngược lại"); - inputArguments[0].name = UA_STRING((char *)"Input Value"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng bật/tắt vùng Muted Safety"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Muted Safety"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Muted Safety"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Muted Safety"), - incAttr, &amr_control::OpcUAServerAPI::writeMutedCallBack, - 1, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::writeMutedCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - *amr_control::OpcUAServerAPI::muted_value_ = *(bool *)input[0].data; - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh Muted Safety gọi thành công"); - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety bị lỗi"); - return UA_STATUSCODE_BAD; -} - -UA_NodeId *amr_control::OpcUAServerAPI::amr_status_str_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::amr_status_en_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ = new UA_NodeId(); -void amr_control::OpcUAServerAPI::addNavigationState(UA_Server *server, UA_NodeId parentID) -{ - UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; - UA_String status_str = UA_STRING_ALLOC((char *)""); - UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); - status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); - status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); - status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Status String"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - status_str_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_str_Id_); - - UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; - UA_Int32 status_en = UA_Int32(0); - UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); - status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); - status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); - status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Status Enum"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - status_en_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_en_Id_); - - UA_VariableAttributes robotFeedBackAttr = UA_VariableAttributes_default; - UA_String feed_back_str = UA_STRING_ALLOC((char *)""); - UA_Variant_setScalar(&robotFeedBackAttr.value, &feed_back_str, &UA_TYPES[UA_TYPES_STRING]); - robotFeedBackAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Feedback"); - robotFeedBackAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Thông tin phản hồi của robot."); - robotFeedBackAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Feedback"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - robotFeedBackAttr, NULL, amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_); - - std::thread([=]() - { - ros::Rate r(10); - while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) - { - r.sleep(); - ros::spinOnce(); - } }) - .detach(); -} - -void amr_control::OpcUAServerAPI::navigationHandle() -{ - if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) - { - if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_ != nullptr) - { - switch (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state) - { - case move_base_core::State::PENDING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PENDING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(0); - break; - case move_base_core::State::ACTIVE: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ACTIVE"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(1); - break; - case move_base_core::State::PREEMPTED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(2); - break; - case move_base_core::State::SUCCEEDED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"SUCCEEDED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(3); - break; - case move_base_core::State::ABORTED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ABORTED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(4); - break; - case move_base_core::State::REJECTED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"REJECTED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(5); - break; - case move_base_core::State::PREEMPTING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(6); - break; - case move_base_core::State::RECALLING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(7); - break; - case move_base_core::State::RECALLED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(8); - break; - case move_base_core::State::LOST: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"LOST"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(9); - break; - case move_base_core::State::PLANNING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PLANNING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(10); - break; - case move_base_core::State::CONTROLLING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CONTROLLING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(11); - break; - case move_base_core::State::CLEARING: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CLEARING"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(12); - break; - case move_base_core::State::PAUSED: - amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PAUSED"); - amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(13); - break; - default: - break; - } - - if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) - amr_control::OpcUAServerAPI::amr_feedback_str_ = UA_STRING_ALLOC((char *)""); - else - amr_control::OpcUAServerAPI::amr_feedback_str_ = - UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->feed_back_str.c_str()); - } - - if (amr_control::OpcUAServerAPI::amr_status_str_Id_ != NULL && amr_control::OpcUAServerAPI::amr_status_en_Id_ != NULL && amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ != NULL) - { - UA_Variant statusStrVariant; - UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::amr_status_str_, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant statusEnVariant; - UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::amr_status_en_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant feedbackVariant; - UA_Variant_setScalar(&feedbackVariant, &amr_control::OpcUAServerAPI::amr_feedback_str_, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_, feedbackVariant); // Truyền đối tượng, không phải con trỏ - } - } -} - -UA_NodeId *amr_control::OpcUAServerAPI::vx_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::vy_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::omega_Id_ = new UA_NodeId(); -void amr_control::OpcUAServerAPI::addVelocityCommand(UA_Server *server, UA_NodeId parentID) -{ - UA_VariableAttributes vxAttr = UA_VariableAttributes_default; - UA_Double vx = UA_Double(0); - UA_Variant_setScalar(&vxAttr.value, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); - vxAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vx"); - vxAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục X."); - vxAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Vx"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - vxAttr, NULL, amr_control::OpcUAServerAPI::vx_Id_); - - UA_VariableAttributes vyAttr = UA_VariableAttributes_default; - UA_Double vy = UA_Double(0); - UA_Variant_setScalar(&vyAttr.value, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); - vyAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vy"); - vyAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục Y."); - vyAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Vy"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - vyAttr, NULL, amr_control::OpcUAServerAPI::vy_Id_); - - UA_VariableAttributes omegaAttr = UA_VariableAttributes_default; - UA_Double angular_vel = UA_Double(0); - UA_Variant_setScalar(&omegaAttr.value, &angular_vel, &UA_TYPES[UA_TYPES_DOUBLE]); - omegaAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Omega"); - omegaAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc quay theo trục Z."); - omegaAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Omega"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - omegaAttr, NULL, amr_control::OpcUAServerAPI::omega_Id_); -} - -void amr_control::OpcUAServerAPI::monitorHandle() -{ - if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) - { - if (amr_control::OpcUAServerAPI::monitor_ptr_ != nullptr) - { - nav_2d_msgs::Twist2D velocity; - if (monitor_ptr_->getVelocity(velocity)) - { - UA_Double vx = std::round(velocity.x * 1000.0) / 1000.0; - UA_Double vy = std::round(velocity.y * 1000.0) / 1000.0; - UA_Double omega = std::round(velocity.theta * 1000.0) / 1000.0; - - if (amr_control::OpcUAServerAPI::vx_Id_ != NULL && - amr_control::OpcUAServerAPI::vy_Id_ != NULL && - amr_control::OpcUAServerAPI::omega_Id_ != NULL) - { - UA_Variant vxVariant; - UA_Variant_setScalar(&vxVariant, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vx_Id_, vxVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant vyVariant; - UA_Variant_setScalar(&vyVariant, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vy_Id_, vyVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant omegaVariant; - UA_Variant_setScalar(&omegaVariant, &omega, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::omega_Id_, omegaVariant); // Truyền đối tượng, không phải con trỏ - } - } - } - } -} - -void amr_control::OpcUAServerAPI::addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng lấy hàng bởi tay máy Dobot"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PickUp"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"PickUp"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"PickUp"), - incAttr, &amr_control::OpcUAServerAPI::PickUpDobotCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::PickUpDobotCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động và chạy tay máy Dobot gọi thành công"); - - if (!amr_control::OpcUAServerAPI::arm_function_ptr_) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); - return UA_STATUSCODE_GOOD; - } - else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) - { - amr_control::OpcUAServerAPI::resetState(); - ROS_INFO("Dobot is running..."); - *amr_control::OpcUAServerAPI::arm_cancel_ = true; - amr_control::OpcUAServerAPI::arm_function_ptr_(); - } - else - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động và chạy tay máy Dobot gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh StartAndRunDobot bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng về gốc tay máy Dobot"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GoHomeDobot"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"GoHomeDobot"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"GoHomeDobot"), - incAttr, &amr_control::OpcUAServerAPI::goHomeDobotCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::goHomeDobotCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh về gốc tay máy Dobot gọi thành công"); - - if (!amr_control::OpcUAServerAPI::arm_function_ptr_) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); - return UA_STATUSCODE_GOOD; - } - else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) - { - amr_control::OpcUAServerAPI::resetState(); - *amr_control::OpcUAServerAPI::arm_go_home_ = true; - *amr_control::OpcUAServerAPI::arm_cancel_ = true; - ROS_INFO("Dobot is Homing..."); - amr_control::OpcUAServerAPI::arm_function_ptr_(); - } - else - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addCountMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument inputArguments[2]; - UA_Argument_init(&inputArguments[0]); - inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng OK cần lấy"); - inputArguments[0].name = UA_STRING((char *)"Count OK"); - inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - inputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&inputArguments[1]); - inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng NG cần lấy"); - inputArguments[1].name = UA_STRING((char *)"Count NG"); - inputArguments[1].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - inputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng thay đổi số lượng hàng muốn lấy"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SetQuantityOfGoods"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"SetQuantityOfGoods"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"SetQuantityOfGoods"), - incAttr, &amr_control::OpcUAServerAPI::counterCallBack, - 2, inputArguments, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::counterCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - - UA_UInt32 count_ok = *(UA_UInt32 *)input[0].data; - UA_UInt32 count_ng = *(UA_UInt32 *)input[1].data; - - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); - - *amr_control::OpcUAServerAPI::count_ok_max_ = count_ok; - *amr_control::OpcUAServerAPI::count_ng_max_ = count_ng; - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addCancelDobotMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng dừng tay máy Dobot"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CancelDobot"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"CancelDobot"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"CancelDobot"), - incAttr, &amr_control::OpcUAServerAPI::cancelDobotCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::cancelDobotCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh dừng tay máy Dobot gọi thành công"); - - *amr_control::OpcUAServerAPI::arm_cancel_ = false; - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addContinueDobotMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng tiếp tục chạy tay máy Dobot"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ContinueDobot"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"ContinueDobot"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"ContinueDobot"), - incAttr, &amr_control::OpcUAServerAPI::continueDobotCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::continueDobotCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); - - amr_control::OpcUAServerAPI::resetState(); - *amr_control::OpcUAServerAPI::arm_continue_ = true; - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng khởi động nguồn tay máy Dobot"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PowerOnDobot"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"PowerOnDobot"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"PowerOnDobot"), - incAttr, &amr_control::OpcUAServerAPI::powerOnDobotCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::powerOnDobotCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động nguồn tay máy Dobot gọi thành công"); - - amr_control::OpcUAServerAPI::resetState(); - *amr_control::OpcUAServerAPI::arm_power_on_ = true; - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot bị lỗi"); - return UA_STATUSCODE_BAD; -} - -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ = new UA_NodeId(); -void amr_control::OpcUAServerAPI::addDobotProperties(UA_Server *server, UA_NodeId parentID) -{ - - UA_VariableAttributes dobot_state_str_Attr = UA_VariableAttributes_default; - UA_String dobot_state_str = UA_STRING_ALLOC((char *)"PENDING"); - UA_Variant_setScalar(&dobot_state_str_Attr.value, &dobot_state_str, &UA_TYPES[UA_TYPES_STRING]); - dobot_state_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State String"); - dobot_state_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); - dobot_state_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"State String"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_state_str_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_); - - UA_VariableAttributes dobot_state_en_Attr = UA_VariableAttributes_default; - UA_UInt32 dobot_state_en = UA_UInt32(amr_control::State::FAILED); - UA_Variant_setScalar(&dobot_state_en_Attr.value, &dobot_state_en, &UA_TYPES[UA_TYPES_UINT32]); - dobot_state_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State Enum"); - dobot_state_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); - dobot_state_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"State Enum"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_state_en_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_); - - UA_VariableAttributes dobot_count_ok_max_Attr = UA_VariableAttributes_default; - UA_UInt32 count_ok_max = *amr_control::OpcUAServerAPI::count_ok_max_; - UA_Variant_setScalar(&dobot_count_ok_max_Attr.value, &count_ok_max, &UA_TYPES[UA_TYPES_UINT32]); - dobot_count_ok_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count OK"); - dobot_count_ok_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng OK cần lấy"); - dobot_count_ok_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Count OK"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_count_ok_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_); - - UA_VariableAttributes dobot_count_ng_max_Attr = UA_VariableAttributes_default; - UA_UInt32 count_ng_max = *amr_control::OpcUAServerAPI::count_ng_max_; - UA_Variant_setScalar(&dobot_count_ng_max_Attr.value, &count_ng_max, &UA_TYPES[UA_TYPES_UINT32]); - dobot_count_ng_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count NG"); - dobot_count_ng_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng NG cần lấy"); - dobot_count_ng_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Count NG"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_count_ng_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_); - - UA_VariableAttributes dobot_mode_ptr_Attr = UA_VariableAttributes_default; - UA_Double *mode = amr_control::OpcUAServerAPI::mode_ptr_; - UA_Variant_setScalar(&dobot_mode_ptr_Attr.value, mode, &UA_TYPES[UA_TYPES_DOUBLE]); - dobot_mode_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Mode"); - dobot_mode_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Chế độ của tay máy Dobot"); - dobot_mode_ptr_Attr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Dobot Mode"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_mode_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); - - UA_VariableAttributes dobot_status_code_ptr_Attr = UA_VariableAttributes_default; - UA_UInt32 *status_code = amr_control::OpcUAServerAPI::status_code_ptr_; - UA_Variant_setScalar(&dobot_status_code_ptr_Attr.value, status_code, &UA_TYPES[UA_TYPES_UINT32]); - dobot_status_code_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Status Code"); - dobot_status_code_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Mã trạng thái của tay máy Dobot"); - dobot_status_code_ptr_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Dobot Status Code"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - dobot_status_code_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); -} - -void amr_control::OpcUAServerAPI::dobotPropertiesHandle() -{ - if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) - { - - if (amr_control::OpcUAServerAPI::arm_thread_ptr_) - { - // if (amr_control::OpcUAServerAPI::arm_joined_) - // { - amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::FINISHED; - amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); - amr_control::OpcUAServerAPI::resetState(); - // } - - // if ( - // !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && - // !amr_control::OpcUAServerAPI::arm_joined_) - // { - // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::INITIALIZING; - // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); - // } - // else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && - // amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && - // !amr_control::OpcUAServerAPI::arm_joined_) - // { - // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::RUNNING; - // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); - // } - } - else - { - amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::WAITING; - amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); - } - - if (amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ != NULL && - amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ != NULL && - amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ != NULL && - amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ != NULL && - amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ != NULL && - amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ != NULL) - { - UA_Variant dobot_state_str_Variant; - UA_Variant_setScalar(&dobot_state_str_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_str_, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_, dobot_state_str_Variant); // Truyền đối tượng, không phải con trỏ - - UA_Variant dobot_state_en_Variant; - UA_Variant_setScalar(&dobot_state_en_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_en_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_, dobot_state_en_Variant); // Truyền đối tượng, không phải con trỏ - - UA_Variant dobot_count_ok_max_Variant; - UA_Variant_setScalar(&dobot_count_ok_max_Variant, &amr_control::OpcUAServerAPI::count_ok_max_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_, dobot_count_ok_max_Variant); // Truyền đối tượng, không phải con trỏ - - UA_Variant dobot_count_ng_max_Variant; - UA_Variant_setScalar(&dobot_count_ng_max_Variant, &amr_control::OpcUAServerAPI::count_ng_max_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_, dobot_count_ng_max_Variant); // Truyền đối tượng, không phải con trỏ - - if (amr_control::OpcUAServerAPI::mode_ptr_) - { - UA_Variant dobot_mode_ptr_Variant; - UA_Variant_setScalar(&dobot_mode_ptr_Variant, amr_control::OpcUAServerAPI::mode_ptr_, &UA_TYPES[UA_TYPES_DOUBLE]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_, dobot_mode_ptr_Variant); - } - - if (amr_control::OpcUAServerAPI::status_code_ptr_) - { - UA_Variant dobot_status_Variant; - UA_Variant_setScalar(&dobot_status_Variant, amr_control::OpcUAServerAPI::status_code_ptr_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_, dobot_status_Variant); - } - } - - if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::RUNNING) - { - if (*(amr_control::OpcUAServerAPI::status_code_ptr_) != amr_control::OpcUAServerAPI::old_status_code_ && - *(amr_control::OpcUAServerAPI::status_code_ptr_) == imr_nova_control::ROBOT_DRAG) - { - amr_control::OpcUAServerAPI::resetState(); - } - } - else if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::FINISHED) - { - *(amr_control::OpcUAServerAPI::status_code_ptr_) = 0; - } - amr_control::OpcUAServerAPI::old_status_code_ = *(amr_control::OpcUAServerAPI::status_code_ptr_); - } -} - -void amr_control::OpcUAServerAPI::addunLoadMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng trả hàng"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"UnLoad"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"UnLoad"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"UnLoad"), - incAttr, &amr_control::OpcUAServerAPI::unLoadCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::unLoadCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh trả hàng gọi thành công"); - - if (!amr_control::OpcUAServerAPI::unLoad_excuted_) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); - return UA_STATUSCODE_GOOD; - } - else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) - { - amr_control::OpcUAServerAPI::unLoad_excuted_(); - } - else - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addLoadMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng nhận hàng"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Load"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"Load"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Load"), - incAttr, &amr_control::OpcUAServerAPI::loadCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::loadCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh nhận hàng gọi thành công"); - - if (!amr_control::OpcUAServerAPI::load_excuted_) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); - return UA_STATUSCODE_GOOD; - } - else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) - { - amr_control::OpcUAServerAPI::load_excuted_(); - } - else - { - UA_Int32 status = STATUS_ERROR; - UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); - return UA_STATUSCODE_GOOD; - } - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh nhận hàng bị lỗi"); - return UA_STATUSCODE_BAD; -} - -void amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID) -{ - /* One output argument */ - UA_Argument outputArguments[2]; - UA_Argument_init(&outputArguments[0]); - outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); - outputArguments[0].name = UA_STRING((char *)"Status"); - outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - outputArguments[0].valueRank = UA_VALUERANK_SCALAR; - - UA_Argument_init(&outputArguments[1]); - outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); - outputArguments[1].name = UA_STRING((char *)"Message"); - outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; - outputArguments[1].valueRank = UA_VALUERANK_SCALAR; - - /* Add the method node */ - UA_MethodAttributes incAttr = UA_MethodAttributes_default; - incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", - (char *)"Chức năng reset"); - incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ResetBelt"); - incAttr.executable = true; - incAttr.userExecutable = true; - UA_Server_addMethodNode(server, - UA_NODEID_STRING(1, (char *)"ResetBelt"), - parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"ResetBelt"), - incAttr, &amr_control::OpcUAServerAPI::resetConveyorBeltCallBack, - 0, NULL, 2, outputArguments, NULL, NULL); -} - -UA_StatusCode amr_control::OpcUAServerAPI::resetConveyorBeltCallBack(UA_Server *server, - const UA_NodeId *sessionId, void *sessionContext, - const UA_NodeId *methodId, void *methodContext, - const UA_NodeId *objectId, void *objectContext, - size_t inputSize, const UA_Variant *input, - size_t outputSize, UA_Variant *output) -{ - UA_Variant_init(output); - UA_Int32 status = STATUS_SUCCESSED; - UA_String message = UA_STRING_ALLOC((char *)"Lệnh reset gọi thành công"); - - *amr_control::OpcUAServerAPI::belt_cancel_ = true; - - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset gọi thành công"); - return UA_STATUSCODE_GOOD; - -STATUSCODE_BADINTERNALERROR: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADINTERNALERROR; - -STATUSCODE_BADREQUESTTOOLARGE: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADREQUESTTOOLARGE; - -STATUSCODE_BADSTRUCTUREMISSING: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - return UA_STATUSCODE_BADSTRUCTUREMISSING; - -STATUSCODE_BAD: - status = STATUS_ERROR; - UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); - UA_Int32_clear(&status); - UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); - UA_String_clear(&message); - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset bị lỗi"); - return UA_STATUSCODE_BAD; -} - -UA_NodeId *amr_control::OpcUAServerAPI::belt_status_str_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::belt_status_en_Id_ = new UA_NodeId(); -UA_NodeId *amr_control::OpcUAServerAPI::have_goods_Id_ = new UA_NodeId(); -void amr_control::OpcUAServerAPI::addConveyorBeltState(UA_Server *server, UA_NodeId parentID) -{ - - UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; - UA_String status_str = UA_STRING_ALLOC((char *)"READY"); - UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); - status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); - status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); - status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Status String"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - status_str_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_str_Id_); - - UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; - UA_Int32 status_en = UA_Int32(*cur_belt_state_en_); - UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); - status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); - status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); - status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Status Enum"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - status_en_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_en_Id_); - - UA_VariableAttributes have_goods_Attr = UA_VariableAttributes_default; - UA_Int32 have_goods = UA_Boolean(false); - UA_Variant_setScalar(&have_goods_Attr.value, &have_goods, &UA_TYPES[UA_TYPES_BOOLEAN]); - have_goods_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Have Goods"); - have_goods_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái đã nhận hàng chưa"); - have_goods_Attr.dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; - UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, - UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), - UA_QUALIFIEDNAME(1, (char *)"Have Goods"), - UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), - have_goods_Attr, NULL, amr_control::OpcUAServerAPI::have_goods_Id_); -} - -void amr_control::OpcUAServerAPI::ConveyorBeltHandle() -{ - if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) - { - switch (*amr_control::OpcUAServerAPI::cur_belt_state_en_) - { - case amr_control::WAITING: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); - break; - case amr_control::INITIALIZING: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); - break; - case amr_control::RUNNING: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); - break; - case amr_control::PAUSED: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"PAUSED"); - break; - case amr_control::FINISHED: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); - break; - case amr_control::FAILED: - amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FAILED"); - break; - default: - break; - } - - if (amr_control::OpcUAServerAPI::belt_status_str_Id_ != NULL && - amr_control::OpcUAServerAPI::belt_status_en_Id_ != NULL && - amr_control::OpcUAServerAPI::have_goods_Id_ != NULL) - { - UA_Variant statusStrVariant; - UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::belt_state_str_, &UA_TYPES[UA_TYPES_STRING]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant statusEnVariant; - UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::cur_belt_state_en_, &UA_TYPES[UA_TYPES_UINT32]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ - - UA_Variant haveGoodsVariant; - UA_Variant_setScalar(&haveGoodsVariant, &amr_control::OpcUAServerAPI::have_goods_, &UA_TYPES[UA_TYPES_BOOLEAN]); - UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::have_goods_Id_, haveGoodsVariant); // Truyền đối tượng, không phải con trỏ - } - } -} - -void *amr_control::OpcUAServerAPI::ThreadWorker(void *_) -{ - ros::Rate rate(10); - - while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) - { - amr_control::OpcUAServerAPI::slamHandle(); - amr_control::OpcUAServerAPI::monitorHandle(); - amr_control::OpcUAServerAPI::navigationHandle(); - amr_control::OpcUAServerAPI::dobotPropertiesHandle(); - amr_control::OpcUAServerAPI::ConveyorBeltHandle(); - - const UA_AsyncOperationRequest *request = NULL; - void *context = NULL; - UA_AsyncOperationType type; - if (UA_Server_getAsyncOperationNonBlocking(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &type, &request, &context, NULL) == true) - { - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Got entry: OKAY"); - - UA_CallMethodResult response = UA_Server_call( - amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &request->callMethodRequest); - - UA_Server_setAsyncOperationResult( - amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), - (UA_AsyncOperationResponse *)&response, context); - - UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Call done: OKAY"); - UA_CallMethodResult_clear(&response); - } - rate.sleep(); - ros::spinOnce(); - } - return 0; -} +// void amr_control::OpcUAServerAPI::addStartMappingMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Bắt đầu quá trình tạo bản đồ "); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartMapping"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"StartMapping"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"StartMapping"), +// incAttr, &amr_control::OpcUAServerAPI::startMappingCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::startMappingCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu tạo bản đồ gọi thành công"); + +// std::thread([=]() +// { +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// amr_control::OpcUAServerAPI::loc_base_ptr_->startMapping(); }) +// .detach(); + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu tạo bản đồ bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addStopMappingMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Kết thúc quá trình tạo bản đồ "); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopMapping"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"StopMapping"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"StopMapping"), +// incAttr, &amr_control::OpcUAServerAPI::stopMappingCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::stopMappingCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); + +// std::thread([=]() +// { +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// amr_control::OpcUAServerAPI::loc_base_ptr_->stopMapping(); }) +// .detach(); + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình tạo bản đồ bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Bắt đầu quá trình định vị bản đồ "); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StartLocalization"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"StartLocalization"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"StartLocalization"), +// incAttr, &amr_control::OpcUAServerAPI::startLocalizationCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::startLocalizationCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ gọi thành công"); + +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// amr_control::OpcUAServerAPI::loc_base_ptr_->startLocalization(); +// else +// { +// message = UA_STRING_ALLOC((char *)"Lệnh bắt đầu định vị bản đồ bị lỗi"); +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh bắt đầu định vị bản đồ bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Kết thúc quá trình định vị bản đồ "); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"StopLocalization"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"StopLocalization"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"StopLocalization"), +// incAttr, &amr_control::OpcUAServerAPI::stopLocalizationCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::stopLocalizationCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); + +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// amr_control::OpcUAServerAPI::loc_base_ptr_->stopLocalization(); +// else +// { +// message = UA_STRING_ALLOC((char *)"Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh kết thúc quá trình định vị bản đồ bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addListMapFilesMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[1]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"List files"); +// outputArguments[0].name = UA_STRING((char *)"List files"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Liệt kê các file bản đồ."); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GetMapFiles"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"GetMapFiles"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"GetMapFiles"), +// incAttr, &amr_control::OpcUAServerAPI::listMapFilesCallBack, +// 0, NULL, 1, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::listMapFilesCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Variant_init(output); +// UA_String message = UA_STRING_ALLOC((char *)""); +// std::stringstream result; +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// { +// amr_control::OpcUAServerAPI::loc_base_ptr_->listMapFiles(result); +// ROS_INFO_STREAM(result.str()); +// message = UA_STRING_ALLOC((char *)result.str().c_str()); +// } + +// UA_Variant_setScalarCopy(output, &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Hoàn thành"); +// return UA_STATUSCODE_GOOD; +// } + +// void amr_control::OpcUAServerAPI::addActivateMapMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One input argument */ +// UA_Argument inputArguments[1]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MapName"); +// inputArguments[0].name = UA_STRING((char *)"MapName"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chọn một map file để định vị bản đồ "); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ActivateMap"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"ActivateMap"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"ActivateMap"), +// incAttr, &amr_control::OpcUAServerAPI::activateMapCallBack, +// 1, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::activateMapCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_String map_name = *(UA_String *)input[0].data; + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ gọi thành công"); +// bool result; +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// { +// std::string map_name_str = std::string(reinterpret_cast(map_name.data), map_name.length); +// if (!map_name_str.empty()) +// result = amr_control::OpcUAServerAPI::loc_base_ptr_->changeStaticMap(map_name_str); +// else +// message = UA_STRING_ALLOC((char *)"MapName chưa có thông tin"); +// } + +// if (!result) +// { +// message = UA_STRING_ALLOC((char *)"Lệnh chọn một map file để định vị bản đồ bị lỗi"); +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh chọn một map file để định vị bản đồ bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[3]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); +// inputArguments[0].name = UA_STRING((char *)"X"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); +// inputArguments[1].name = UA_STRING((char *)"Y"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[2]); +// inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"YAW"); +// inputArguments[2].name = UA_STRING((char *)"YAW"); +// inputArguments[2].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[2].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Set vị trí ban đầu của robot trên bản đồ."); + +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Initial Pose"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Initial Pose"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Initial Pose"), +// incAttr, &amr_control::OpcUAServerAPI::setInitialPoseCallBack, +// 3, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::setInitialPoseCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Double X = *(UA_Double *)input[0].data; +// UA_Double Y = *(UA_Double *)input[1].data; +// UA_Double Yaw = *(UA_Double *)input[2].data; + +// std::string fixed_frame = "map"; +// geometry_msgs::PoseWithCovarianceStamped pose; +// pose.header.frame_id = fixed_frame; +// pose.header.stamp = ros::Time::now(); + +// // set x,y coord +// pose.pose.pose.position.x = X; +// pose.pose.pose.position.y = Y; +// pose.pose.pose.position.z = 0.0; + +// // set theta +// tf::Quaternion quat; +// quat.setRPY(0.0, 0.0, Yaw); +// tf::quaternionTFToMsg(quat, pose.pose.pose.orientation); + +// pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5; +// pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5; +// pose.pose.covariance[6 * 5 + 5] = M_PI / 12.0 * M_PI / 12.0; + +// amr_control::OpcUAServerAPI::init_pub_.publish(pose); + +// UA_Variant_init(output); + +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Initial Pose gọi thành công"); + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Initial Pose bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// UA_NodeId *amr_control::OpcUAServerAPI::robotPoseX_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::robotPoseY_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::robotPoseYaw_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::currentActiveMap_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::slamState_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::workingDirectory_Id_ = new UA_NodeId(); +// void amr_control::OpcUAServerAPI::addRobotPoseProperty(UA_Server *server, UA_NodeId parentID) +// { +// UA_NodeId robotPose_Id; /* get the nodeid assigned by the server */ +// UA_ObjectAttributes slamAttr = UA_ObjectAttributes_default; +// slamAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RobotPose"); +// UA_Server_addObjectNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES), +// UA_QUALIFIEDNAME(1, (char *)"RobotPose"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEOBJECTTYPE), +// slamAttr, NULL, &robotPose_Id); + +// // UA_NodeId robotPoseX_Id_; +// UA_VariableAttributes poseXAttr = UA_VariableAttributes_default; +// UA_Double pose_x = 0; +// UA_Variant_setScalar(&poseXAttr.value, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); +// poseXAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"X"); +// poseXAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ X của robot."); +// poseXAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"X"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// poseXAttr, NULL, amr_control::OpcUAServerAPI::robotPoseX_Id_); + +// // UA_NodeId robotPoseY_Id_; +// UA_VariableAttributes poseYAttr = UA_VariableAttributes_default; +// UA_Double pose_y = 0; +// UA_Variant_setScalar(&poseYAttr.value, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); +// poseYAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Y"); +// poseYAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Y của robot."); +// poseYAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Y"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// poseYAttr, NULL, amr_control::OpcUAServerAPI::robotPoseY_Id_); + +// // UA_NodeId robotPoseYaw_Id_; +// UA_VariableAttributes poseYawAttr = UA_VariableAttributes_default; +// UA_Double pose_yaw = 0; +// UA_Variant_setScalar(&poseYawAttr.value, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); +// poseYawAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Yaw"); +// poseYawAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến thông tin tọa độ Yaw của robot."); +// poseYawAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, robotPose_Id, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Yaw"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// poseYawAttr, NULL, amr_control::OpcUAServerAPI::robotPoseYaw_Id_); + +// // UA_NodeId currentActiveMap_Id; +// UA_VariableAttributes currentActiveMapAttr = UA_VariableAttributes_default; +// UA_String current_active_map = UA_STRING_ALLOC((char *)" "); +// UA_Variant_setScalar(¤tActiveMapAttr.value, ¤t_active_map, &UA_TYPES[UA_TYPES_STRING]); +// currentActiveMapAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CurrentActiveMap"); +// currentActiveMapAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến lưu trữ thông tin về map đang được active."); +// currentActiveMapAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// currentActiveMapAttr, NULL, amr_control::OpcUAServerAPI::currentActiveMap_Id_); + +// // UA_NodeId slamState_Id_; +// UA_VariableAttributes slamStateAttr = UA_VariableAttributes_default; +// UA_String slam_state = UA_STRING_ALLOC((char *)" "); +// UA_Variant_setScalar(&slamStateAttr.value, &slam_state, &UA_TYPES[UA_TYPES_STRING]); +// slamStateAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SlamState"); +// slamStateAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến trạng thái của module slam: Mapping, Localization, Calibrations, Ready, Error"); +// slamStateAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"CurrentActiveMap"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// slamStateAttr, NULL, amr_control::OpcUAServerAPI::slamState_Id_); + +// // UA_NodeId workingDirectory_Id_ +// UA_VariableAttributes workingDirectoryAttr = UA_VariableAttributes_default; +// UA_String working_directory = UA_STRING_ALLOC((char *)" "); +// UA_Variant_setScalar(&workingDirectoryAttr.value, &working_directory, &UA_TYPES[UA_TYPES_STRING]); +// workingDirectoryAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"WorkingDirectories"); +// workingDirectoryAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Đường dẫn lưu trữ các file bản đồ"); +// workingDirectoryAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"WorkingDirectories"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// workingDirectoryAttr, NULL, amr_control::OpcUAServerAPI::workingDirectory_Id_); +// } + +// void amr_control::OpcUAServerAPI::slamHandle() +// { +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) +// { +// geometry_msgs::Pose2D robot_pose; +// if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(robot_pose)) +// { +// UA_LOG_WARNING(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "getRobotPose: Is Not OKAY"); +// } +// else +// { +// UA_Double pose_x = std::round(robot_pose.x * 10000.0) / 10000.0; +// UA_Double pose_y = std::round(robot_pose.y * 10000.0) / 10000.0; +// UA_Double pose_yaw = std::round(robot_pose.theta * 10000.0) / 10000.0; + +// if (amr_control::OpcUAServerAPI::robotPoseX_Id_ != NULL && +// amr_control::OpcUAServerAPI::robotPoseY_Id_ != NULL && +// amr_control::OpcUAServerAPI::robotPoseYaw_Id_ != NULL && +// amr_control::OpcUAServerAPI::currentActiveMap_Id_ != NULL && +// amr_control::OpcUAServerAPI::slamState_Id_ != NULL && +// amr_control::OpcUAServerAPI::workingDirectory_Id_ != NULL) +// { +// // Cập nhật giá trị X +// UA_Variant poseXVariant; +// UA_Variant_setScalar(&poseXVariant, &pose_x, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseX_Id_, poseXVariant); // Truyền đối tượng, không phải con trỏ + +// // Cập nhật giá trị Y +// UA_Variant poseYVariant; +// UA_Variant_setScalar(&poseYVariant, &pose_y, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseY_Id_, poseYVariant); // Truyền đối tượng, không phải con trỏ + +// // Cập nhật giá trị Yaw +// UA_Variant poseYawVariant; +// UA_Variant_setScalar(&poseYawVariant, &pose_yaw, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::robotPoseYaw_Id_, poseYawVariant); + +// if (amr_control::OpcUAServerAPI::loc_base_ptr_) +// { +// // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); +// // ROS_INFO_THROTTLE(2.0, "%s", amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); +// UA_Variant currentActiveMapVariant; +// UA_String activated_map_filename = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->activated_map_filename_.c_str()); +// UA_Variant_setScalar(¤tActiveMapVariant, &activated_map_filename, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::currentActiveMap_Id_, currentActiveMapVariant); + +// UA_Variant workingDirectoryvariant; +// UA_String working_dir = UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::loc_base_ptr_->working_dir_.c_str()); +// UA_Variant_setScalar(&workingDirectoryvariant, &working_dir, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::workingDirectory_Id_, workingDirectoryvariant); + +// UA_String slam_state = UA_STRING_ALLOC((char *)" "); +// switch (amr_control::OpcUAServerAPI::loc_base_ptr_->getState()) +// { +// case loc_core::Mapping: +// slam_state = UA_STRING_ALLOC((char *)"Mapping"); +// break; +// case loc_core::Localization: +// slam_state = UA_STRING_ALLOC((char *)"Localization"); +// break; +// case loc_core::Calibrations: +// slam_state = UA_STRING_ALLOC((char *)"Calibrations"); +// break; +// case loc_core::Ready: +// slam_state = UA_STRING_ALLOC((char *)"Ready"); +// break; +// case loc_core::Error: +// slam_state = UA_STRING_ALLOC((char *)"Error"); +// break; +// default: +// break; +// } +// UA_Variant slamStatevariant; +// UA_Variant_setScalar(&slamStatevariant, &slam_state, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::slamState_Id_, slamStatevariant); +// } +// } +// } +// } +// } + +// void amr_control::OpcUAServerAPI::addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[2]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); +// inputArguments[0].name = UA_STRING((char *)"Length"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); +// inputArguments[1].name = UA_STRING((char *)"Nodes"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; +// inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; +// inputArguments[1].arrayDimensionsSize = 2; +// inputArguments[1].arrayDimensions = pInputDimension; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message với độ chính xác mong muốn."); + +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToNode"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"MoveToNode"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"MoveToNode"), +// incAttr, &amr_control::OpcUAServerAPI::moveToNodeCallBack, +// 2, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::moveToNodeCallBack( +// UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_UInt32 length = *(UA_UInt32 *)input[0].data; +// UA_Double *nodes = (UA_Double *)input[1].data; + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"MoveToNode gọi thành công"); + +// if (input[1].arrayDimensionsSize != 2) +// return UA_STATUSCODE_BADINTERNALERROR; + +// const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; +// const int row = input[1].arrayDimensions[1]; + +// if (length > column) +// { +// message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); +// goto STATUSCODE_BADREQUESTTOOLARGE; +// } + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// UA_Double matrix[column][row]; +// for (int i = 0; i < column; i++) +// for (int j = 0; j < row; j++) +// matrix[i][j] = nodes[i * row + j]; + +// geometry_msgs::Pose2D goal_2d; +// goal_2d.x = matrix[column - 1][0]; +// goal_2d.y = matrix[column - 1][1]; +// goal_2d.theta = matrix[column - 1][2]; +// const double velocity = matrix[column - 1][3]; +// const double xy_tolerance = matrix[column - 1][4]; +// const double yaw_tolerance = 0.025; +// const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); +// amr_control::OpcUAServerAPI::move_base_ptr_->moveTo(goal, xy_tolerance, yaw_tolerance); +// amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; +// amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; +// geometry_msgs::Vector3 linear; +// linear.x = velocity; +// amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addDockToNodeMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[3]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Kích thước N"); +// inputArguments[0].name = UA_STRING((char *)"Length"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Ma trận [Nx5][x, y, yaw, vmax, accuracy]"); +// inputArguments[1].name = UA_STRING((char *)"Nodes"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_UInt32 pInputDimension[2] = {static_cast(3), 5}; +// inputArguments[1].valueRank = UA_VALUERANK_TWO_DIMENSIONS; +// inputArguments[1].arrayDimensionsSize = 2; +// inputArguments[1].arrayDimensions = pInputDimension; + +// UA_Argument_init(&inputArguments[2]); +// inputArguments[2].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Maker"); +// inputArguments[2].name = UA_STRING((char *)"Maker"); +// inputArguments[2].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// inputArguments[2].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là lấy hàng ở một vị trí lận cận với điểm đích được order."); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveToPickup"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"MoveToPickup"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"MoveToPickup"), +// incAttr, &amr_control::OpcUAServerAPI::dockToNodeCallBack, +// 2, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::dockToNodeCallBack( +// UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_UInt32 length = *(UA_UInt32 *)input[0].data; +// UA_Double *nodes = (UA_Double *)input[1].data; +// UA_String maker = *(UA_String *)input[2].data; + + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"MoveToPickUp gọi thành công"); + +// if (input[1].arrayDimensionsSize != 2) +// return UA_STATUSCODE_BADINTERNALERROR; + +// const int column = length < input[1].arrayDimensions[0] ? length : input[1].arrayDimensions[0]; +// const int row = input[1].arrayDimensions[1]; + +// if (length > column) +// { +// message = UA_STRING_ALLOC((char *)"Lỗi vì biến 'Length' vượt quá ngưỡng cho phép là 500000 phần tử"); +// goto STATUSCODE_BADREQUESTTOOLARGE; +// } + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// UA_Double matrix[column][row]; +// for (int i = 0; i < column; i++) +// for (int j = 0; j < row; j++) +// matrix[i][j] = nodes[i * row + j]; + +// geometry_msgs::Pose2D goal_2d; +// goal_2d.x = matrix[column - 1][0]; +// goal_2d.y = matrix[column - 1][1]; +// goal_2d.theta = matrix[column - 1][2]; +// const double velocity = matrix[column - 1][3]; +// const double xy_tolerance = matrix[column - 1][4]; +// const double yaw_tolerance = 0.025; +// const geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", ros::Time::now()); +// std::string maker_str(reinterpret_cast(maker.data), maker.length); +// amr_control::OpcUAServerAPI::move_base_ptr_->dockTo(maker_str, goal, xy_tolerance, yaw_tolerance); +// amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; +// amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; +// geometry_msgs::Vector3 linear; +// linear.x = velocity; +// amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToPickUp gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addRotateToMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[2]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Góc muốn quay trong giải từ -Pi đến Pi"); +// inputArguments[0].name = UA_STRING((char *)"Yaw goal"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Sai số góc cho phép"); +// inputArguments[1].name = UA_STRING((char *)"Yaw tolerance"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_INT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Robot quay tại chỗ một góc yaw."); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"RotateTo"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"RotateTo"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"RotateTo"), +// incAttr, &amr_control::OpcUAServerAPI::rotateToCallBack, +// 2, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::rotateToCallBack( +// UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Double Yaw = *(UA_Double *)input[0].data; +// UA_Double tolerance = *(UA_Double *)input[1].data; +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Successed"); +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// geometry_msgs::PoseStamped goal; +// if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(goal)) +// { +// message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); +// goto STATUSCODE_BADINTERNALERROR; +// } +// tf2::Quaternion q; +// q.setRPY(0, 0, angles::normalize_angle(Yaw)); +// goal.pose.orientation.x = q.x(); +// goal.pose.orientation.y = q.y(); +// goal.pose.orientation.z = q.z(); +// goal.pose.orientation.w = q.w(); +// amr_control::OpcUAServerAPI::move_base_ptr_->rotateTo(goal, tolerance); +// } +// else +// { +// return UA_STATUSCODE_BADSTRUCTUREMISSING; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo was called"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BAD: +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "RotateTo bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addMoveStraightMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[2]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Khoảng cách đi '+' là tiến '-' là lùi"); +// inputArguments[0].name = UA_STRING((char *)"Distance"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc di chuyển"); +// inputArguments[1].name = UA_STRING((char *)"Velocity linear"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Di chuyển đến một điểm đích theo thông tin đường đi có trong order message.\nNhiệm vụ là trả hàng ở một vị trí lận cận với điểm đích được order."); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"MoveStraight"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"MoveStraight"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"MoveStraight"), +// incAttr, &amr_control::OpcUAServerAPI::moveStraighCallBack, +// 2, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::moveStraighCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Double distance = *(UA_Double *)input[0].data; +// UA_Double velocity = *(UA_Double *)input[1].data; + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"MoveStraight gọi thành công"); + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// geometry_msgs::PoseStamped pose; +// if (!amr_control::OpcUAServerAPI::move_base_ptr_->getRobotPose(pose)) +// { +// message = UA_STRING_ALLOC((char *)"Lỗi không nhận được tọa độ"); +// goto STATUSCODE_BADINTERNALERROR; +// } +// ROS_INFO_STREAM(pose); +// geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); +// amr_control::OpcUAServerAPI::move_base_ptr_->moveStraightTo(goal); +// amr_control::OpcUAServerAPI::cmd_vel_max_.x = velocity; +// amr_control::OpcUAServerAPI::cmd_vel_max_.theta = 0.4; +// geometry_msgs::Vector3 linear; +// linear.x = velocity; +// amr_control::OpcUAServerAPI::move_base_ptr_->setTwistLinear(linear); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng di chuyển đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveStraight gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "MoveToNode bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addCancelMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Hủy lệnh di chuyển của AMR"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Cancel"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Cancel"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Cancel"), +// incAttr, &amr_control::OpcUAServerAPI::cancelCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::cancelCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Cancel gọi thành công"); + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// amr_control::OpcUAServerAPI::move_base_ptr_->cancel(); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng hủy lệnh đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Cancel bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addPauseMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Lệnh tạm dừng di chuyển của AMR"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Pause"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Pause"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Pause"), +// incAttr, &amr_control::OpcUAServerAPI::pauseCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::pauseCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Pause gọi thành công"); + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// amr_control::OpcUAServerAPI::move_base_ptr_->pause(); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng tạm dừng đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Pause bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addResumeMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Lệnh tiếp tục di chuyển của AMR"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Resume"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Resume"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Resume"), +// incAttr, &amr_control::OpcUAServerAPI::resumeCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::resumeCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Resume gọi thành công"); + +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr) +// { +// amr_control::OpcUAServerAPI::move_base_ptr_->resume(); +// } +// else +// { +// message = UA_STRING_ALLOC((char *)"Chức năng tiếp tục đang bị lỗi khởi tạo"); +// goto STATUSCODE_BADSTRUCTUREMISSING; +// } +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Resume bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addResetMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Lệnh reset AMR"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Reset"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Reset"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Reset"), +// incAttr, &amr_control::OpcUAServerAPI::resetCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::resetCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// // sleep(5); + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Reset gọi thành công"); + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Reset bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addWriteMutedMethod(UA_Server *server, UA_NodeId parentID) +// { +// UA_Argument inputArguments[1]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Tích là bật hoặc ngược lại"); +// inputArguments[0].name = UA_STRING((char *)"Input Value"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng bật/tắt vùng Muted Safety"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Muted Safety"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Muted Safety"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Muted Safety"), +// incAttr, &amr_control::OpcUAServerAPI::writeMutedCallBack, +// 1, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::writeMutedCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// *amr_control::OpcUAServerAPI::muted_value_ = *(bool *)input[0].data; + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh Muted Safety gọi thành công"); + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh Muted Safety bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// UA_NodeId *amr_control::OpcUAServerAPI::amr_status_str_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::amr_status_en_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ = new UA_NodeId(); +// void amr_control::OpcUAServerAPI::addNavigationState(UA_Server *server, UA_NodeId parentID) +// { +// UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; +// UA_String status_str = UA_STRING_ALLOC((char *)""); +// UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); +// status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); +// status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); +// status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Status String"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// status_str_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_str_Id_); + +// UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; +// UA_Int32 status_en = UA_Int32(0); +// UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); +// status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); +// status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của robot."); +// status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Status Enum"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// status_en_Attr, NULL, amr_control::OpcUAServerAPI::amr_status_en_Id_); + +// UA_VariableAttributes robotFeedBackAttr = UA_VariableAttributes_default; +// UA_String feed_back_str = UA_STRING_ALLOC((char *)""); +// UA_Variant_setScalar(&robotFeedBackAttr.value, &feed_back_str, &UA_TYPES[UA_TYPES_STRING]); +// robotFeedBackAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Feedback"); +// robotFeedBackAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Thông tin phản hồi của robot."); +// robotFeedBackAttr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Feedback"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// robotFeedBackAttr, NULL, amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_); + +// std::thread([=]() +// { +// ros::Rate r(10); +// while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) +// { +// r.sleep(); +// ros::spinOnce(); +// } }) +// .detach(); +// } + +// void amr_control::OpcUAServerAPI::navigationHandle() +// { +// if (amr_control::OpcUAServerAPI::move_base_ptr_ != nullptr && amr_control::OpcUAServerAPI::server_ptr_ != nullptr) +// { +// if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_ != nullptr) +// { +// switch (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state) +// { +// case move_base_core::State::PENDING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PENDING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(0); +// break; +// case move_base_core::State::ACTIVE: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ACTIVE"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(1); +// break; +// case move_base_core::State::PREEMPTED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(2); +// break; +// case move_base_core::State::SUCCEEDED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"SUCCEEDED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(3); +// break; +// case move_base_core::State::ABORTED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"ABORTED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(4); +// break; +// case move_base_core::State::REJECTED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"REJECTED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(5); +// break; +// case move_base_core::State::PREEMPTING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PREEMPTING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(6); +// break; +// case move_base_core::State::RECALLING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(7); +// break; +// case move_base_core::State::RECALLED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"RECALLED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(8); +// break; +// case move_base_core::State::LOST: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"LOST"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(9); +// break; +// case move_base_core::State::PLANNING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PLANNING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(10); +// break; +// case move_base_core::State::CONTROLLING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CONTROLLING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(11); +// break; +// case move_base_core::State::CLEARING: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"CLEARING"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(12); +// break; +// case move_base_core::State::PAUSED: +// amr_control::OpcUAServerAPI::amr_status_str_ = UA_STRING_ALLOC((char *)"PAUSED"); +// amr_control::OpcUAServerAPI::amr_status_en_ = UA_UInt32(13); +// break; +// default: +// break; +// } + +// if (amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) +// amr_control::OpcUAServerAPI::amr_feedback_str_ = UA_STRING_ALLOC((char *)""); +// else +// amr_control::OpcUAServerAPI::amr_feedback_str_ = +// UA_STRING_ALLOC((char *)amr_control::OpcUAServerAPI::move_base_ptr_->nav_feedback_->feed_back_str.c_str()); +// } + +// if (amr_control::OpcUAServerAPI::amr_status_str_Id_ != NULL && amr_control::OpcUAServerAPI::amr_status_en_Id_ != NULL && amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_ != NULL) +// { +// UA_Variant statusStrVariant; +// UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::amr_status_str_, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant statusEnVariant; +// UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::amr_status_en_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant feedbackVariant; +// UA_Variant_setScalar(&feedbackVariant, &amr_control::OpcUAServerAPI::amr_feedback_str_, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::amr_robotFeedBack_Id_, feedbackVariant); // Truyền đối tượng, không phải con trỏ +// } +// } +// } + +// UA_NodeId *amr_control::OpcUAServerAPI::vx_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::vy_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::omega_Id_ = new UA_NodeId(); +// void amr_control::OpcUAServerAPI::addVelocityCommand(UA_Server *server, UA_NodeId parentID) +// { +// UA_VariableAttributes vxAttr = UA_VariableAttributes_default; +// UA_Double vx = UA_Double(0); +// UA_Variant_setScalar(&vxAttr.value, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); +// vxAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vx"); +// vxAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục X."); +// vxAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Vx"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// vxAttr, NULL, amr_control::OpcUAServerAPI::vx_Id_); + +// UA_VariableAttributes vyAttr = UA_VariableAttributes_default; +// UA_Double vy = UA_Double(0); +// UA_Variant_setScalar(&vyAttr.value, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); +// vyAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vy"); +// vyAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc tịnh tiến theo trục Y."); +// vyAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Vy"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// vyAttr, NULL, amr_control::OpcUAServerAPI::vy_Id_); + +// UA_VariableAttributes omegaAttr = UA_VariableAttributes_default; +// UA_Double angular_vel = UA_Double(0); +// UA_Variant_setScalar(&omegaAttr.value, &angular_vel, &UA_TYPES[UA_TYPES_DOUBLE]); +// omegaAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Omega"); +// omegaAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Vận tốc quay theo trục Z."); +// omegaAttr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Omega"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// omegaAttr, NULL, amr_control::OpcUAServerAPI::omega_Id_); +// } + +// void amr_control::OpcUAServerAPI::monitorHandle() +// { +// if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) +// { +// if (amr_control::OpcUAServerAPI::monitor_ptr_ != nullptr) +// { +// nav_2d_msgs::Twist2D velocity; +// if (monitor_ptr_->getVelocity(velocity)) +// { +// UA_Double vx = std::round(velocity.x * 1000.0) / 1000.0; +// UA_Double vy = std::round(velocity.y * 1000.0) / 1000.0; +// UA_Double omega = std::round(velocity.theta * 1000.0) / 1000.0; + +// if (amr_control::OpcUAServerAPI::vx_Id_ != NULL && +// amr_control::OpcUAServerAPI::vy_Id_ != NULL && +// amr_control::OpcUAServerAPI::omega_Id_ != NULL) +// { +// UA_Variant vxVariant; +// UA_Variant_setScalar(&vxVariant, &vx, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vx_Id_, vxVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant vyVariant; +// UA_Variant_setScalar(&vyVariant, &vy, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::vy_Id_, vyVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant omegaVariant; +// UA_Variant_setScalar(&omegaVariant, &omega, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::omega_Id_, omegaVariant); // Truyền đối tượng, không phải con trỏ +// } +// } +// } +// } +// } + +// void amr_control::OpcUAServerAPI::addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng lấy hàng bởi tay máy Dobot"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PickUp"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"PickUp"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"PickUp"), +// incAttr, &amr_control::OpcUAServerAPI::PickUpDobotCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::PickUpDobotCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động và chạy tay máy Dobot gọi thành công"); + +// if (!amr_control::OpcUAServerAPI::arm_function_ptr_) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } +// if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); +// return UA_STATUSCODE_GOOD; +// } +// else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) +// { +// amr_control::OpcUAServerAPI::resetState(); +// ROS_INFO("Dobot is running..."); +// *amr_control::OpcUAServerAPI::arm_cancel_ = true; +// amr_control::OpcUAServerAPI::arm_function_ptr_(); +// } +// else +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động và chạy tay máy Dobot gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh StartAndRunDobot bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng về gốc tay máy Dobot"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"GoHomeDobot"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"GoHomeDobot"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"GoHomeDobot"), +// incAttr, &amr_control::OpcUAServerAPI::goHomeDobotCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::goHomeDobotCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh về gốc tay máy Dobot gọi thành công"); + +// if (!amr_control::OpcUAServerAPI::arm_function_ptr_) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Dobot đang bận"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Dobot đang bận"); +// return UA_STATUSCODE_GOOD; +// } +// else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable()) +// { +// amr_control::OpcUAServerAPI::resetState(); +// *amr_control::OpcUAServerAPI::arm_go_home_ = true; +// *amr_control::OpcUAServerAPI::arm_cancel_ = true; +// ROS_INFO("Dobot is Homing..."); +// amr_control::OpcUAServerAPI::arm_function_ptr_(); +// } +// else +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh về gốc tay máy Dobot bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addCountMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument inputArguments[2]; +// UA_Argument_init(&inputArguments[0]); +// inputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng OK cần lấy"); +// inputArguments[0].name = UA_STRING((char *)"Count OK"); +// inputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// inputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&inputArguments[1]); +// inputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Biến số lượng hàng NG cần lấy"); +// inputArguments[1].name = UA_STRING((char *)"Count NG"); +// inputArguments[1].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// inputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng thay đổi số lượng hàng muốn lấy"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"SetQuantityOfGoods"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"SetQuantityOfGoods"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"SetQuantityOfGoods"), +// incAttr, &amr_control::OpcUAServerAPI::counterCallBack, +// 2, inputArguments, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::counterCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { + +// UA_UInt32 count_ok = *(UA_UInt32 *)input[0].data; +// UA_UInt32 count_ng = *(UA_UInt32 *)input[1].data; + +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); + +// *amr_control::OpcUAServerAPI::count_ok_max_ = count_ok; +// *amr_control::OpcUAServerAPI::count_ng_max_ = count_ng; + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Chức năng thay đổi số lượng hàng OK muốn lấy gọi bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addCancelDobotMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng dừng tay máy Dobot"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"CancelDobot"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"CancelDobot"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"CancelDobot"), +// incAttr, &amr_control::OpcUAServerAPI::cancelDobotCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::cancelDobotCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh dừng tay máy Dobot gọi thành công"); + +// *amr_control::OpcUAServerAPI::arm_cancel_ = false; + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh dừng tay máy Dobot bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addContinueDobotMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng tiếp tục chạy tay máy Dobot"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ContinueDobot"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"ContinueDobot"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"ContinueDobot"), +// incAttr, &amr_control::OpcUAServerAPI::continueDobotCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::continueDobotCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); + +// amr_control::OpcUAServerAPI::resetState(); +// *amr_control::OpcUAServerAPI::arm_continue_ = true; + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh tiếp tục chạy tay máy Dobot bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng khởi động nguồn tay máy Dobot"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"PowerOnDobot"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"PowerOnDobot"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"PowerOnDobot"), +// incAttr, &amr_control::OpcUAServerAPI::powerOnDobotCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::powerOnDobotCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh khởi động nguồn tay máy Dobot gọi thành công"); + +// amr_control::OpcUAServerAPI::resetState(); +// *amr_control::OpcUAServerAPI::arm_power_on_ = true; + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh khởi động nguồn tay máy Dobot bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ = new UA_NodeId(); +// void amr_control::OpcUAServerAPI::addDobotProperties(UA_Server *server, UA_NodeId parentID) +// { + +// UA_VariableAttributes dobot_state_str_Attr = UA_VariableAttributes_default; +// UA_String dobot_state_str = UA_STRING_ALLOC((char *)"PENDING"); +// UA_Variant_setScalar(&dobot_state_str_Attr.value, &dobot_state_str, &UA_TYPES[UA_TYPES_STRING]); +// dobot_state_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State String"); +// dobot_state_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); +// dobot_state_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"State String"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_state_str_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_); + +// UA_VariableAttributes dobot_state_en_Attr = UA_VariableAttributes_default; +// UA_UInt32 dobot_state_en = UA_UInt32(amr_control::State::FAILED); +// UA_Variant_setScalar(&dobot_state_en_Attr.value, &dobot_state_en, &UA_TYPES[UA_TYPES_UINT32]); +// dobot_state_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"State Enum"); +// dobot_state_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Dobot."); +// dobot_state_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"State Enum"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_state_en_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_); + +// UA_VariableAttributes dobot_count_ok_max_Attr = UA_VariableAttributes_default; +// UA_UInt32 count_ok_max = *amr_control::OpcUAServerAPI::count_ok_max_; +// UA_Variant_setScalar(&dobot_count_ok_max_Attr.value, &count_ok_max, &UA_TYPES[UA_TYPES_UINT32]); +// dobot_count_ok_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count OK"); +// dobot_count_ok_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng OK cần lấy"); +// dobot_count_ok_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Count OK"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_count_ok_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_); + +// UA_VariableAttributes dobot_count_ng_max_Attr = UA_VariableAttributes_default; +// UA_UInt32 count_ng_max = *amr_control::OpcUAServerAPI::count_ng_max_; +// UA_Variant_setScalar(&dobot_count_ng_max_Attr.value, &count_ng_max, &UA_TYPES[UA_TYPES_UINT32]); +// dobot_count_ng_max_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Count NG"); +// dobot_count_ng_max_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Số lượng hàng NG cần lấy"); +// dobot_count_ng_max_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Count NG"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_count_ng_max_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_); + +// UA_VariableAttributes dobot_mode_ptr_Attr = UA_VariableAttributes_default; +// UA_Double *mode = amr_control::OpcUAServerAPI::mode_ptr_; +// UA_Variant_setScalar(&dobot_mode_ptr_Attr.value, mode, &UA_TYPES[UA_TYPES_DOUBLE]); +// dobot_mode_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Mode"); +// dobot_mode_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Chế độ của tay máy Dobot"); +// dobot_mode_ptr_Attr.dataType = UA_TYPES[UA_TYPES_DOUBLE].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Dobot Mode"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_mode_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); + +// UA_VariableAttributes dobot_status_code_ptr_Attr = UA_VariableAttributes_default; +// UA_UInt32 *status_code = amr_control::OpcUAServerAPI::status_code_ptr_; +// UA_Variant_setScalar(&dobot_status_code_ptr_Attr.value, status_code, &UA_TYPES[UA_TYPES_UINT32]); +// dobot_status_code_ptr_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Dobot Status Code"); +// dobot_status_code_ptr_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Mã trạng thái của tay máy Dobot"); +// dobot_status_code_ptr_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Dobot Status Code"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// dobot_status_code_ptr_Attr, NULL, amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_); +// } + +// void amr_control::OpcUAServerAPI::dobotPropertiesHandle() +// { +// if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) +// { + +// if (amr_control::OpcUAServerAPI::arm_thread_ptr_) +// { +// // if (amr_control::OpcUAServerAPI::arm_joined_) +// // { +// amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::FINISHED; +// amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); +// amr_control::OpcUAServerAPI::resetState(); +// // } + +// // if ( +// // !amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && +// // !amr_control::OpcUAServerAPI::arm_joined_) +// // { +// // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::INITIALIZING; +// // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); +// // } +// // else if (amr_control::OpcUAServerAPI::arm_thread_ptr_ && +// // amr_control::OpcUAServerAPI::arm_thread_ptr_->joinable() && +// // !amr_control::OpcUAServerAPI::arm_joined_) +// // { +// // amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::RUNNING; +// // amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); +// // } +// } +// else +// { +// amr_control::OpcUAServerAPI::arm_dobot_state_en_ = amr_control::State::WAITING; +// amr_control::OpcUAServerAPI::arm_dobot_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); +// } + +// if (amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_ != NULL && +// amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_ != NULL && +// amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_ != NULL && +// amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_ != NULL && +// amr_control::OpcUAServerAPI::arm_dobot_mode_Id_ != NULL && +// amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_ != NULL) +// { +// UA_Variant dobot_state_str_Variant; +// UA_Variant_setScalar(&dobot_state_str_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_str_, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_str_Id_, dobot_state_str_Variant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant dobot_state_en_Variant; +// UA_Variant_setScalar(&dobot_state_en_Variant, &amr_control::OpcUAServerAPI::arm_dobot_state_en_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_state_en_Id_, dobot_state_en_Variant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant dobot_count_ok_max_Variant; +// UA_Variant_setScalar(&dobot_count_ok_max_Variant, &amr_control::OpcUAServerAPI::count_ok_max_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ok_max_Id_, dobot_count_ok_max_Variant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant dobot_count_ng_max_Variant; +// UA_Variant_setScalar(&dobot_count_ng_max_Variant, &amr_control::OpcUAServerAPI::count_ng_max_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_count_ng_max_Id_, dobot_count_ng_max_Variant); // Truyền đối tượng, không phải con trỏ + +// if (amr_control::OpcUAServerAPI::mode_ptr_) +// { +// UA_Variant dobot_mode_ptr_Variant; +// UA_Variant_setScalar(&dobot_mode_ptr_Variant, amr_control::OpcUAServerAPI::mode_ptr_, &UA_TYPES[UA_TYPES_DOUBLE]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_mode_Id_, dobot_mode_ptr_Variant); +// } + +// if (amr_control::OpcUAServerAPI::status_code_ptr_) +// { +// UA_Variant dobot_status_Variant; +// UA_Variant_setScalar(&dobot_status_Variant, amr_control::OpcUAServerAPI::status_code_ptr_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::arm_dobot_status_code_Id_, dobot_status_Variant); +// } +// } + +// if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::RUNNING) +// { +// if (*(amr_control::OpcUAServerAPI::status_code_ptr_) != amr_control::OpcUAServerAPI::old_status_code_ && +// *(amr_control::OpcUAServerAPI::status_code_ptr_) == imr_nova_control::ROBOT_DRAG) +// { +// amr_control::OpcUAServerAPI::resetState(); +// } +// } +// else if (amr_control::OpcUAServerAPI::arm_dobot_state_en_ == amr_control::State::FINISHED) +// { +// *(amr_control::OpcUAServerAPI::status_code_ptr_) = 0; +// } +// amr_control::OpcUAServerAPI::old_status_code_ = *(amr_control::OpcUAServerAPI::status_code_ptr_); +// } +// } + +// void amr_control::OpcUAServerAPI::addunLoadMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng trả hàng"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"UnLoad"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"UnLoad"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"UnLoad"), +// incAttr, &amr_control::OpcUAServerAPI::unLoadCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::unLoadCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh trả hàng gọi thành công"); + +// if (!amr_control::OpcUAServerAPI::unLoad_excuted_) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); +// return UA_STATUSCODE_GOOD; +// } +// else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) +// { +// amr_control::OpcUAServerAPI::unLoad_excuted_(); +// } +// else +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addLoadMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng nhận hàng"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Load"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"Load"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Load"), +// incAttr, &amr_control::OpcUAServerAPI::loadCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::loadCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh nhận hàng gọi thành công"); + +// if (!amr_control::OpcUAServerAPI::load_excuted_) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Bang Tai đang bận"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Bang Tai đang bận"); +// return UA_STATUSCODE_GOOD; +// } +// else if (amr_control::OpcUAServerAPI::belt_thread_ptr_ && !amr_control::OpcUAServerAPI::belt_thread_ptr_->joinable()) +// { +// amr_control::OpcUAServerAPI::load_excuted_(); +// } +// else +// { +// UA_Int32 status = STATUS_ERROR; +// UA_String message = UA_STRING_ALLOC((char *)"Lỗi khi khởi tạo nhiệm vụ"); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lỗi khi khởi tạo nhiệm vụ"); +// return UA_STATUSCODE_GOOD; +// } + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh trả hàng gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh nhận hàng bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// void amr_control::OpcUAServerAPI::addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID) +// { +// /* One output argument */ +// UA_Argument outputArguments[2]; +// UA_Argument_init(&outputArguments[0]); +// outputArguments[0].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status"); +// outputArguments[0].name = UA_STRING((char *)"Status"); +// outputArguments[0].dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// outputArguments[0].valueRank = UA_VALUERANK_SCALAR; + +// UA_Argument_init(&outputArguments[1]); +// outputArguments[1].description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Message"); +// outputArguments[1].name = UA_STRING((char *)"Message"); +// outputArguments[1].dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// outputArguments[1].valueRank = UA_VALUERANK_SCALAR; + +// /* Add the method node */ +// UA_MethodAttributes incAttr = UA_MethodAttributes_default; +// incAttr.description = UA_LOCALIZEDTEXT((char *)"en-US", +// (char *)"Chức năng reset"); +// incAttr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"ResetBelt"); +// incAttr.executable = true; +// incAttr.userExecutable = true; +// UA_Server_addMethodNode(server, +// UA_NODEID_STRING(1, (char *)"ResetBelt"), +// parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"ResetBelt"), +// incAttr, &amr_control::OpcUAServerAPI::resetConveyorBeltCallBack, +// 0, NULL, 2, outputArguments, NULL, NULL); +// } + +// UA_StatusCode amr_control::OpcUAServerAPI::resetConveyorBeltCallBack(UA_Server *server, +// const UA_NodeId *sessionId, void *sessionContext, +// const UA_NodeId *methodId, void *methodContext, +// const UA_NodeId *objectId, void *objectContext, +// size_t inputSize, const UA_Variant *input, +// size_t outputSize, UA_Variant *output) +// { +// UA_Variant_init(output); +// UA_Int32 status = STATUS_SUCCESSED; +// UA_String message = UA_STRING_ALLOC((char *)"Lệnh reset gọi thành công"); + +// *amr_control::OpcUAServerAPI::belt_cancel_ = true; + +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset gọi thành công"); +// return UA_STATUSCODE_GOOD; + +// STATUSCODE_BADINTERNALERROR: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADINTERNALERROR; + +// STATUSCODE_BADREQUESTTOOLARGE: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADREQUESTTOOLARGE; + +// STATUSCODE_BADSTRUCTUREMISSING: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// return UA_STATUSCODE_BADSTRUCTUREMISSING; + +// STATUSCODE_BAD: +// status = STATUS_ERROR; +// UA_Variant_setScalarCopy(&output[0], &status, &UA_TYPES[UA_TYPES_INT32]); +// UA_Int32_clear(&status); +// UA_Variant_setScalarCopy(&output[1], &message, &UA_TYPES[UA_TYPES_STRING]); +// UA_String_clear(&message); +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "Lệnh reset bị lỗi"); +// return UA_STATUSCODE_BAD; +// } + +// UA_NodeId *amr_control::OpcUAServerAPI::belt_status_str_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::belt_status_en_Id_ = new UA_NodeId(); +// UA_NodeId *amr_control::OpcUAServerAPI::have_goods_Id_ = new UA_NodeId(); +// void amr_control::OpcUAServerAPI::addConveyorBeltState(UA_Server *server, UA_NodeId parentID) +// { + +// UA_VariableAttributes status_str_Attr = UA_VariableAttributes_default; +// UA_String status_str = UA_STRING_ALLOC((char *)"READY"); +// UA_Variant_setScalar(&status_str_Attr.value, &status_str, &UA_TYPES[UA_TYPES_STRING]); +// status_str_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status String"); +// status_str_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); +// status_str_Attr.dataType = UA_TYPES[UA_TYPES_STRING].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Status String"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// status_str_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_str_Id_); + +// UA_VariableAttributes status_en_Attr = UA_VariableAttributes_default; +// UA_Int32 status_en = UA_Int32(*cur_belt_state_en_); +// UA_Variant_setScalar(&status_en_Attr.value, &status_en, &UA_TYPES[UA_TYPES_UINT32]); +// status_en_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Status Enum"); +// status_en_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái của Conveyor Belt."); +// status_en_Attr.dataType = UA_TYPES[UA_TYPES_UINT32].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Status Enum"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// status_en_Attr, NULL, amr_control::OpcUAServerAPI::belt_status_en_Id_); + +// UA_VariableAttributes have_goods_Attr = UA_VariableAttributes_default; +// UA_Int32 have_goods = UA_Boolean(false); +// UA_Variant_setScalar(&have_goods_Attr.value, &have_goods, &UA_TYPES[UA_TYPES_BOOLEAN]); +// have_goods_Attr.displayName = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Have Goods"); +// have_goods_Attr.description = UA_LOCALIZEDTEXT((char *)"en-US", (char *)"Trạng thái đã nhận hàng chưa"); +// have_goods_Attr.dataType = UA_TYPES[UA_TYPES_BOOLEAN].typeId; +// UA_Server_addVariableNode(server, UA_NODEID_NULL, parentID, +// UA_NODEID_NUMERIC(0, UA_NS0ID_HASCOMPONENT), +// UA_QUALIFIEDNAME(1, (char *)"Have Goods"), +// UA_NODEID_NUMERIC(0, UA_NS0ID_BASEDATAVARIABLETYPE), +// have_goods_Attr, NULL, amr_control::OpcUAServerAPI::have_goods_Id_); +// } + +// void amr_control::OpcUAServerAPI::ConveyorBeltHandle() +// { +// if (amr_control::OpcUAServerAPI::server_ptr_ != nullptr) +// { +// switch (*amr_control::OpcUAServerAPI::cur_belt_state_en_) +// { +// case amr_control::WAITING: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"WAITING"); +// break; +// case amr_control::INITIALIZING: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"INITIALIZING"); +// break; +// case amr_control::RUNNING: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"RUNNING"); +// break; +// case amr_control::PAUSED: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"PAUSED"); +// break; +// case amr_control::FINISHED: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FINISHED"); +// break; +// case amr_control::FAILED: +// amr_control::OpcUAServerAPI::belt_state_str_ = UA_STRING_ALLOC((char *)"FAILED"); +// break; +// default: +// break; +// } + +// if (amr_control::OpcUAServerAPI::belt_status_str_Id_ != NULL && +// amr_control::OpcUAServerAPI::belt_status_en_Id_ != NULL && +// amr_control::OpcUAServerAPI::have_goods_Id_ != NULL) +// { +// UA_Variant statusStrVariant; +// UA_Variant_setScalar(&statusStrVariant, &amr_control::OpcUAServerAPI::belt_state_str_, &UA_TYPES[UA_TYPES_STRING]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_str_Id_, statusStrVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant statusEnVariant; +// UA_Variant_setScalar(&statusEnVariant, &amr_control::OpcUAServerAPI::cur_belt_state_en_, &UA_TYPES[UA_TYPES_UINT32]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::belt_status_en_Id_, statusEnVariant); // Truyền đối tượng, không phải con trỏ + +// UA_Variant haveGoodsVariant; +// UA_Variant_setScalar(&haveGoodsVariant, &amr_control::OpcUAServerAPI::have_goods_, &UA_TYPES[UA_TYPES_BOOLEAN]); +// UA_Server_writeValue(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), *amr_control::OpcUAServerAPI::have_goods_Id_, haveGoodsVariant); // Truyền đối tượng, không phải con trỏ +// } +// } +// } + +// void *amr_control::OpcUAServerAPI::ThreadWorker(void *_) +// { +// ros::Rate rate(10); + +// while (ros::ok() && amr_control::OpcUAServerAPI::server_ptr_->isRunning()) +// { +// amr_control::OpcUAServerAPI::slamHandle(); +// amr_control::OpcUAServerAPI::monitorHandle(); +// amr_control::OpcUAServerAPI::navigationHandle(); +// amr_control::OpcUAServerAPI::dobotPropertiesHandle(); +// amr_control::OpcUAServerAPI::ConveyorBeltHandle(); + +// const UA_AsyncOperationRequest *request = NULL; +// void *context = NULL; +// UA_AsyncOperationType type; +// if (UA_Server_getAsyncOperationNonBlocking(amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &type, &request, &context, NULL) == true) +// { +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Got entry: OKAY"); + +// UA_CallMethodResult response = UA_Server_call( +// amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), &request->callMethodRequest); + +// UA_Server_setAsyncOperationResult( +// amr_control::OpcUAServerAPI::server_ptr_->getServerObject(), +// (UA_AsyncOperationResponse *)&response, context); + +// UA_LOG_INFO(UA_Log_Stdout, UA_LOGCATEGORY_SERVER, "AsyncMethod_Testing: Call done: OKAY"); +// UA_CallMethodResult_clear(&response); +// } +// rate.sleep(); +// ros::spinOnce(); +// } +// return 0; +// } void amr_control::OpcUAServerAPI::resetState() { diff --git a/Controllers/Packages/amr_startup/CMakeLists.txt b/Controllers/Packages/amr_startup/CMakeLists.txt deleted file mode 100644 index b2c0f81..0000000 --- a/Controllers/Packages/amr_startup/CMakeLists.txt +++ /dev/null @@ -1,205 +0,0 @@ -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) diff --git a/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml deleted file mode 100644 index 853cace..0000000 --- a/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml +++ /dev/null @@ -1,48 +0,0 @@ -#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 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_common_params.yaml b/Controllers/Packages/amr_startup/config/costmap_common_params.yaml deleted file mode 100755 index af52a10..0000000 --- a/Controllers/Packages/amr_startup/config/costmap_common_params.yaml +++ /dev/null @@ -1,54 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params.yaml deleted file mode 100755 index ae6d769..0000000 --- a/Controllers/Packages/amr_startup/config/costmap_global_params.yaml +++ /dev/null @@ -1,13 +0,0 @@ -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. - diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml deleted file mode 100755 index 06d0037..0000000 --- a/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ /dev/null @@ -1,10 +0,0 @@ -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 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_local_params.yaml b/Controllers/Packages/amr_startup/config/costmap_local_params.yaml deleted file mode 100755 index 49d126f..0000000 --- a/Controllers/Packages/amr_startup/config/costmap_local_params.yaml +++ /dev/null @@ -1,16 +0,0 @@ -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 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml deleted file mode 100755 index 6ef480b..0000000 --- a/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ /dev/null @@ -1,8 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/config/custom_global_params.yaml b/Controllers/Packages/amr_startup/config/custom_global_params.yaml deleted file mode 100644 index 6536a6e..0000000 --- a/Controllers/Packages/amr_startup/config/custom_global_params.yaml +++ /dev/null @@ -1,17 +0,0 @@ -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" - diff --git a/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml deleted file mode 100755 index b8d766a..0000000 --- a/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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 /odom diff --git a/Controllers/Packages/amr_startup/config/ekf.yaml b/Controllers/Packages/amr_startup/config/ekf.yaml deleted file mode 100755 index 7344ee3..0000000 --- a/Controllers/Packages/amr_startup/config/ekf.yaml +++ /dev/null @@ -1,217 +0,0 @@ -# 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::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] diff --git a/Controllers/Packages/amr_startup/config/localization.yaml b/Controllers/Packages/amr_startup/config/localization.yaml deleted file mode 100644 index 677cb96..0000000 --- a/Controllers/Packages/amr_startup/config/localization.yaml +++ /dev/null @@ -1,36 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/config/maker_sources.yaml b/Controllers/Packages/amr_startup/config/maker_sources.yaml deleted file mode 100644 index 5dd2276..0000000 --- a/Controllers/Packages/amr_startup/config/maker_sources.yaml +++ /dev/null @@ -1,120 +0,0 @@ -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 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/mapping.yaml b/Controllers/Packages/amr_startup/config/mapping.yaml deleted file mode 100644 index e58fce3..0000000 --- a/Controllers/Packages/amr_startup/config/mapping.yaml +++ /dev/null @@ -1,63 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/config/move_base_common_params.yaml b/Controllers/Packages/amr_startup/config/move_base_common_params.yaml deleted file mode 100755 index 6d275d8..0000000 --- a/Controllers/Packages/amr_startup/config/move_base_common_params.yaml +++ /dev/null @@ -1,24 +0,0 @@ - -### 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 - - diff --git a/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml deleted file mode 100755 index aa6a592..0000000 --- a/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml +++ /dev/null @@ -1,106 +0,0 @@ -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 - diff --git a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m deleted file mode 100755 index 2810593..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m +++ /dev/null @@ -1,280 +0,0 @@ -% /* -% * 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'); diff --git a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py deleted file mode 100755 index c8801c0..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py +++ /dev/null @@ -1,416 +0,0 @@ -#!/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) diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim deleted file mode 100755 index cb56cd0..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim +++ /dev/null @@ -1,1203 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 80 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0000 0.0452 1.5708 --0.0000 0.0904 1.5708 --0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim deleted file mode 100755 index f27907b..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 5 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim deleted file mode 100755 index 0f11e5d..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 200 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim deleted file mode 100755 index 596f5c5..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim +++ /dev/null @@ -1,2403 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 160 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: 16 4 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0904 0.0095 0.0349 -0.1806 0.0222 0.0699 -0.2707 0.0381 0.1048 -0.3604 0.0572 0.1398 -0.4496 0.0795 0.1747 -0.5383 0.1050 0.2097 -0.6264 0.1335 0.2446 -0.7136 0.1652 0.2796 -0.8000 0.2000 0.3145 -primID: 3 -startangle_c: 0 -endpose_c: 16 -4 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0904 -0.0095 -0.0349 -0.1806 -0.0222 -0.0699 -0.2707 -0.0381 -0.1048 -0.3604 -0.0572 -0.1398 -0.4496 -0.0795 -0.1747 -0.5383 -0.1050 -0.2097 -0.6264 -0.1335 -0.2446 -0.7136 -0.1652 -0.2796 -0.8000 -0.2000 -0.3145 -primID: 4 -startangle_c: 0 -endpose_c: 5 2 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0280 0.0085 0.0223 -0.0559 0.0177 0.0445 -0.0839 0.0275 0.0668 -0.1117 0.0380 0.0890 -0.1396 0.0491 0.1113 -0.1673 0.0609 0.1335 -0.1950 0.0733 0.1558 -0.2225 0.0863 0.1781 -0.2500 0.1000 0.2003 -primID: 5 -startangle_c: 0 -endpose_c: 5 -2 -1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0280 -0.0085 -0.0223 -0.0559 -0.0177 -0.0445 -0.0839 -0.0275 -0.0668 -0.1117 -0.0380 -0.0890 -0.1396 -0.0491 -0.1113 -0.1673 -0.0609 -0.1335 -0.1950 -0.0733 -0.1558 -0.2225 -0.0863 -0.1781 -0.2500 -0.1000 -0.2003 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 7 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 8 -startangle_c: 0 -endpose_c: 8 1 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0056 0.0000 -0.0889 0.0111 0.0000 -0.1333 0.0167 0.0000 -0.1778 0.0222 0.0000 -0.2222 0.0278 0.0000 -0.2667 0.0333 0.0000 -0.3111 0.0389 0.0000 -0.3556 0.0444 0.0000 -0.4000 0.0500 0.0000 -primID: 9 -startangle_c: 0 -endpose_c: 8 -1 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 -0.0056 0.0000 -0.0889 -0.0111 0.0000 -0.1333 -0.0167 0.0000 -0.1778 -0.0222 0.0000 -0.2222 -0.0278 0.0000 -0.2667 -0.0333 0.0000 -0.3111 -0.0389 0.0000 -0.3556 -0.0444 0.0000 -0.4000 -0.0500 0.0000 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: 13 9 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0793 0.0378 0.4329 -0.1572 0.0787 0.4732 -0.2334 0.1229 0.5134 -0.3079 0.1701 0.5537 -0.3805 0.2204 0.5939 -0.4512 0.2736 0.6342 -0.5197 0.3296 0.6744 -0.5860 0.3885 0.7147 -0.6500 0.4500 0.7549 -primID: 3 -startangle_c: 1 -endpose_c: 14 3 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0741 0.0305 0.3774 -0.1493 0.0582 0.3302 -0.2256 0.0824 0.2830 -0.3031 0.1030 0.2359 -0.3814 0.1199 0.1887 -0.4604 0.1330 0.1415 -0.5400 0.1424 0.0943 -0.6199 0.1481 0.0472 -0.7000 0.1500 -0.0000 -primID: 4 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 5 -startangle_c: 1 -endpose_c: 6 1 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0320 0.0105 0.3551 -0.0644 0.0197 0.3174 -0.0973 0.0278 0.2798 -0.1304 0.0346 0.2421 -0.1639 0.0402 0.2045 -0.1977 0.0446 0.1669 -0.2316 0.0476 0.1292 -0.2658 0.0495 0.0916 -0.3000 0.0500 0.0540 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 7 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 8 -startangle_c: 1 -endpose_c: 7 2 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0389 0.0111 0.3927 -0.0778 0.0222 0.3927 -0.1167 0.0333 0.3927 -0.1556 0.0444 0.3927 -0.1944 0.0556 0.3927 -0.2333 0.0667 0.3927 -0.2722 0.0778 0.3927 -0.3111 0.0889 0.3927 -0.3500 0.1000 0.3927 -primID: 9 -startangle_c: 1 -endpose_c: 5 4 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0278 0.0222 0.3927 -0.0556 0.0444 0.3927 -0.0833 0.0667 0.3927 -0.1111 0.0889 0.3927 -0.1389 0.1111 0.3927 -0.1667 0.1333 0.3927 -0.1944 0.1556 0.3927 -0.2222 0.1778 0.3927 -0.2500 0.2000 0.3927 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: 11 14 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0705 0.0705 0.7854 -0.1411 0.1411 0.7854 -0.2116 0.2116 0.7854 -0.2816 0.2828 0.8201 -0.3469 0.3581 0.8917 -0.4068 0.4379 0.9633 -0.4607 0.5218 1.0349 -0.5086 0.6093 1.1065 -0.5500 0.7000 1.1781 -primID: 3 -startangle_c: 2 -endpose_c: 14 11 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0705 0.0705 0.7854 -0.1411 0.1411 0.7854 -0.2116 0.2116 0.7854 -0.2828 0.2816 0.7507 -0.3581 0.3469 0.6791 -0.4379 0.4068 0.6075 -0.5218 0.4607 0.5359 -0.6093 0.5086 0.4643 -0.7000 0.5500 0.3927 -primID: 4 -startangle_c: 2 -endpose_c: 4 6 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0278 0.0292 0.8288 -0.0543 0.0595 0.8722 -0.0795 0.0910 0.9156 -0.1033 0.1235 0.9590 -0.1257 0.1571 1.0024 -0.1466 0.1916 1.0458 -0.1659 0.2269 1.0892 -0.1838 0.2631 1.1326 -0.2000 0.3000 1.1760 -primID: 5 -startangle_c: 2 -endpose_c: 6 4 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0292 0.0278 0.7420 -0.0595 0.0543 0.6986 -0.0910 0.0795 0.6552 -0.1235 0.1033 0.6118 -0.1571 0.1257 0.5684 -0.1916 0.1466 0.5250 -0.2269 0.1659 0.4816 -0.2631 0.1838 0.4382 -0.3000 0.2000 0.3948 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 7 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 8 -startangle_c: 2 -endpose_c: 6 7 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0389 0.7854 -0.0667 0.0778 0.7854 -0.1000 0.1167 0.7854 -0.1333 0.1556 0.7854 -0.1667 0.1944 0.7854 -0.2000 0.2333 0.7854 -0.2333 0.2722 0.7854 -0.2667 0.3111 0.7854 -0.3000 0.3500 0.7854 -primID: 9 -startangle_c: 2 -endpose_c: 7 6 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0389 0.0333 0.7854 -0.0778 0.0667 0.7854 -0.1167 0.1000 0.7854 -0.1556 0.1333 0.7854 -0.1944 0.1667 0.7854 -0.2333 0.2000 0.7854 -0.2722 0.2333 0.7854 -0.3111 0.2667 0.7854 -0.3500 0.3000 0.7854 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: 9 13 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0378 0.0793 1.1378 -0.0787 0.1572 1.0976 -0.1229 0.2334 1.0574 -0.1701 0.3079 1.0171 -0.2204 0.3805 0.9769 -0.2736 0.4512 0.9366 -0.3296 0.5197 0.8964 -0.3885 0.5860 0.8561 -0.4500 0.6500 0.8159 -primID: 3 -startangle_c: 3 -endpose_c: 3 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0305 0.0741 1.1934 -0.0582 0.1493 1.2406 -0.0824 0.2256 1.2878 -0.1030 0.3031 1.3349 -0.1199 0.3814 1.3821 -0.1330 0.4604 1.4293 -0.1424 0.5400 1.4765 -0.1481 0.6199 1.5236 -0.1500 0.7000 1.5708 -primID: 4 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 5 -startangle_c: 3 -endpose_c: 1 6 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0105 0.0320 1.2157 -0.0197 0.0644 1.2534 -0.0278 0.0973 1.2910 -0.0346 0.1304 1.3286 -0.0402 0.1639 1.3663 -0.0446 0.1977 1.4039 -0.0476 0.2316 1.4416 -0.0495 0.2658 1.4792 -0.0500 0.3000 1.5168 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 7 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 8 -startangle_c: 3 -endpose_c: 2 7 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0389 1.1781 -0.0222 0.0778 1.1781 -0.0333 0.1167 1.1781 -0.0444 0.1556 1.1781 -0.0556 0.1944 1.1781 -0.0667 0.2333 1.1781 -0.0778 0.2722 1.1781 -0.0889 0.3111 1.1781 -0.1000 0.3500 1.1781 -primID: 9 -startangle_c: 3 -endpose_c: 4 5 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0222 0.0278 1.1781 -0.0444 0.0556 1.1781 -0.0667 0.0833 1.1781 -0.0889 0.1111 1.1781 -0.1111 0.1389 1.1781 -0.1333 0.1667 1.1781 -0.1556 0.1944 1.1781 -0.1778 0.2222 1.1781 -0.2000 0.2500 1.1781 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: -4 16 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0095 0.0904 1.6057 --0.0222 0.1806 1.6407 --0.0381 0.2707 1.6756 --0.0572 0.3604 1.7106 --0.0795 0.4496 1.7455 --0.1050 0.5383 1.7805 --0.1335 0.6264 1.8154 --0.1652 0.7136 1.8503 --0.2000 0.8000 1.8853 -primID: 3 -startangle_c: 4 -endpose_c: 4 16 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0095 0.0904 1.5359 -0.0222 0.1806 1.5009 -0.0381 0.2707 1.4660 -0.0572 0.3604 1.4310 -0.0795 0.4496 1.3961 -0.1050 0.5383 1.3611 -0.1335 0.6264 1.3262 -0.1652 0.7136 1.2912 -0.2000 0.8000 1.2563 -primID: 4 -startangle_c: 4 -endpose_c: -2 5 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0085 0.0280 1.5931 --0.0177 0.0559 1.6153 --0.0275 0.0839 1.6376 --0.0380 0.1117 1.6598 --0.0491 0.1396 1.6821 --0.0609 0.1673 1.7043 --0.0733 0.1950 1.7266 --0.0863 0.2225 1.7489 --0.1000 0.2500 1.7711 -primID: 5 -startangle_c: 4 -endpose_c: 2 5 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0085 0.0280 1.5485 -0.0177 0.0559 1.5263 -0.0275 0.0839 1.5040 -0.0380 0.1117 1.4818 -0.0491 0.1396 1.4595 -0.0609 0.1673 1.4373 -0.0733 0.1950 1.4150 -0.0863 0.2225 1.3927 -0.1000 0.2500 1.3705 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 7 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 8 -startangle_c: 4 -endpose_c: -1 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0056 0.0444 1.5708 --0.0111 0.0889 1.5708 --0.0167 0.1333 1.5708 --0.0222 0.1778 1.5708 --0.0278 0.2222 1.5708 --0.0333 0.2667 1.5708 --0.0389 0.3111 1.5708 --0.0444 0.3556 1.5708 --0.0500 0.4000 1.5708 -primID: 9 -startangle_c: 4 -endpose_c: 1 8 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0056 0.0444 1.5708 -0.0111 0.0889 1.5708 -0.0167 0.1333 1.5708 -0.0222 0.1778 1.5708 -0.0278 0.2222 1.5708 -0.0333 0.2667 1.5708 -0.0389 0.3111 1.5708 -0.0444 0.3556 1.5708 -0.0500 0.4000 1.5708 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: -9 13 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0378 0.0793 2.0037 --0.0787 0.1572 2.0440 --0.1229 0.2334 2.0842 --0.1701 0.3079 2.1245 --0.2204 0.3805 2.1647 --0.2736 0.4512 2.2050 --0.3296 0.5197 2.2452 --0.3885 0.5860 2.2855 --0.4500 0.6500 2.3257 -primID: 3 -startangle_c: 5 -endpose_c: -3 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0305 0.0741 1.9482 --0.0582 0.1493 1.9010 --0.0824 0.2256 1.8538 --0.1030 0.3031 1.8067 --0.1199 0.3814 1.7595 --0.1330 0.4604 1.7123 --0.1424 0.5400 1.6651 --0.1481 0.6199 1.6180 --0.1500 0.7000 1.5708 -primID: 4 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 5 -startangle_c: 5 -endpose_c: -1 6 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0105 0.0320 1.9259 --0.0197 0.0644 1.8882 --0.0278 0.0973 1.8506 --0.0346 0.1304 1.8129 --0.0402 0.1639 1.7753 --0.0446 0.1977 1.7377 --0.0476 0.2316 1.7000 --0.0495 0.2658 1.6624 --0.0500 0.3000 1.6248 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 7 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 8 -startangle_c: 5 -endpose_c: -2 7 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0389 1.9635 --0.0222 0.0778 1.9635 --0.0333 0.1167 1.9635 --0.0444 0.1556 1.9635 --0.0556 0.1944 1.9635 --0.0667 0.2333 1.9635 --0.0778 0.2722 1.9635 --0.0889 0.3111 1.9635 --0.1000 0.3500 1.9635 -primID: 9 -startangle_c: 5 -endpose_c: -4 5 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0222 0.0278 1.9635 --0.0444 0.0556 1.9635 --0.0667 0.0833 1.9635 --0.0889 0.1111 1.9635 --0.1111 0.1389 1.9635 --0.1333 0.1667 1.9635 --0.1556 0.1944 1.9635 --0.1778 0.2222 1.9635 --0.2000 0.2500 1.9635 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: -14 11 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0705 0.0705 2.3562 --0.1411 0.1411 2.3562 --0.2116 0.2116 2.3562 --0.2828 0.2816 2.3909 --0.3581 0.3469 2.4625 --0.4379 0.4068 2.5341 --0.5218 0.4607 2.6057 --0.6093 0.5086 2.6773 --0.7000 0.5500 2.7489 -primID: 3 -startangle_c: 6 -endpose_c: -11 14 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0705 0.0705 2.3562 --0.1411 0.1411 2.3562 --0.2116 0.2116 2.3562 --0.2816 0.2828 2.3215 --0.3469 0.3581 2.2499 --0.4068 0.4379 2.1783 --0.4607 0.5218 2.1067 --0.5086 0.6093 2.0351 --0.5500 0.7000 1.9635 -primID: 4 -startangle_c: 6 -endpose_c: -6 4 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0292 0.0278 2.3996 --0.0595 0.0543 2.4430 --0.0910 0.0795 2.4864 --0.1235 0.1033 2.5298 --0.1571 0.1257 2.5732 --0.1916 0.1466 2.6166 --0.2269 0.1659 2.6600 --0.2631 0.1838 2.7034 --0.3000 0.2000 2.7468 -primID: 5 -startangle_c: 6 -endpose_c: -4 6 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0278 0.0292 2.3128 --0.0543 0.0595 2.2694 --0.0795 0.0910 2.2260 --0.1033 0.1235 2.1826 --0.1257 0.1571 2.1392 --0.1466 0.1916 2.0958 --0.1659 0.2269 2.0524 --0.1838 0.2631 2.0090 --0.2000 0.3000 1.9656 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 7 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 8 -startangle_c: 6 -endpose_c: -7 6 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0389 0.0333 2.3562 --0.0778 0.0667 2.3562 --0.1167 0.1000 2.3562 --0.1556 0.1333 2.3562 --0.1944 0.1667 2.3562 --0.2333 0.2000 2.3562 --0.2722 0.2333 2.3562 --0.3111 0.2667 2.3562 --0.3500 0.3000 2.3562 -primID: 9 -startangle_c: 6 -endpose_c: -6 7 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0389 2.3562 --0.0667 0.0778 2.3562 --0.1000 0.1167 2.3562 --0.1333 0.1556 2.3562 --0.1667 0.1944 2.3562 --0.2000 0.2333 2.3562 --0.2333 0.2722 2.3562 --0.2667 0.3111 2.3562 --0.3000 0.3500 2.3562 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: -13 9 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0793 0.0378 2.7086 --0.1572 0.0787 2.6684 --0.2334 0.1229 2.6281 --0.3079 0.1701 2.5879 --0.3805 0.2204 2.5477 --0.4512 0.2736 2.5074 --0.5197 0.3296 2.4672 --0.5860 0.3885 2.4269 --0.6500 0.4500 2.3867 -primID: 3 -startangle_c: 7 -endpose_c: -14 3 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0741 0.0305 2.7642 --0.1493 0.0582 2.8114 --0.2256 0.0824 2.8586 --0.3031 0.1030 2.9057 --0.3814 0.1199 2.9529 --0.4604 0.1330 3.0001 --0.5400 0.1424 3.0472 --0.6199 0.1481 3.0944 --0.7000 0.1500 3.1416 -primID: 4 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 5 -startangle_c: 7 -endpose_c: -6 1 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0320 0.0105 2.7865 --0.0644 0.0197 2.8242 --0.0973 0.0278 2.8618 --0.1304 0.0346 2.8994 --0.1639 0.0402 2.9371 --0.1977 0.0446 2.9747 --0.2316 0.0476 3.0124 --0.2658 0.0495 3.0500 --0.3000 0.0500 3.0876 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 7 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 8 -startangle_c: 7 -endpose_c: -7 2 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0389 0.0111 2.7489 --0.0778 0.0222 2.7489 --0.1167 0.0333 2.7489 --0.1556 0.0444 2.7489 --0.1944 0.0556 2.7489 --0.2333 0.0667 2.7489 --0.2722 0.0778 2.7489 --0.3111 0.0889 2.7489 --0.3500 0.1000 2.7489 -primID: 9 -startangle_c: 7 -endpose_c: -5 4 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0278 0.0222 2.7489 --0.0556 0.0444 2.7489 --0.0833 0.0667 2.7489 --0.1111 0.0889 2.7489 --0.1389 0.1111 2.7489 --0.1667 0.1333 2.7489 --0.1944 0.1556 2.7489 --0.2222 0.1778 2.7489 --0.2500 0.2000 2.7489 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: -16 -4 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0904 -0.0095 3.1765 --0.1806 -0.0222 3.2115 --0.2707 -0.0381 3.2464 --0.3604 -0.0572 3.2814 --0.4496 -0.0795 3.3163 --0.5383 -0.1050 3.3513 --0.6264 -0.1335 3.3862 --0.7136 -0.1652 3.4211 --0.8000 -0.2000 3.4561 -primID: 3 -startangle_c: 8 -endpose_c: -16 4 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0904 0.0095 3.1066 --0.1806 0.0222 3.0717 --0.2707 0.0381 3.0368 --0.3604 0.0572 3.0018 --0.4496 0.0795 2.9669 --0.5383 0.1050 2.9319 --0.6264 0.1335 2.8970 --0.7136 0.1652 2.8620 --0.8000 0.2000 2.8271 -primID: 4 -startangle_c: 8 -endpose_c: -5 -2 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0280 -0.0085 3.1639 --0.0559 -0.0177 3.1861 --0.0839 -0.0275 3.2084 --0.1117 -0.0380 3.2306 --0.1396 -0.0491 3.2529 --0.1673 -0.0609 3.2751 --0.1950 -0.0733 3.2974 --0.2225 -0.0863 3.3197 --0.2500 -0.1000 3.3419 -primID: 5 -startangle_c: 8 -endpose_c: -5 2 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0280 0.0085 3.1193 --0.0559 0.0177 3.0971 --0.0839 0.0275 3.0748 --0.1117 0.0380 3.0526 --0.1396 0.0491 3.0303 --0.1673 0.0609 3.0080 --0.1950 0.0733 2.9858 --0.2225 0.0863 2.9635 --0.2500 0.1000 2.9413 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 7 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 8 -startangle_c: 8 -endpose_c: -8 -1 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 -0.0056 3.1416 --0.0889 -0.0111 3.1416 --0.1333 -0.0167 3.1416 --0.1778 -0.0222 3.1416 --0.2222 -0.0278 3.1416 --0.2667 -0.0333 3.1416 --0.3111 -0.0389 3.1416 --0.3556 -0.0444 3.1416 --0.4000 -0.0500 3.1416 -primID: 9 -startangle_c: 8 -endpose_c: -8 1 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0056 3.1416 --0.0889 0.0111 3.1416 --0.1333 0.0167 3.1416 --0.1778 0.0222 3.1416 --0.2222 0.0278 3.1416 --0.2667 0.0333 3.1416 --0.3111 0.0389 3.1416 --0.3556 0.0444 3.1416 --0.4000 0.0500 3.1416 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: -13 -9 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0793 -0.0378 3.5745 --0.1572 -0.0787 3.6148 --0.2334 -0.1229 3.6550 --0.3079 -0.1701 3.6953 --0.3805 -0.2204 3.7355 --0.4512 -0.2736 3.7758 --0.5197 -0.3296 3.8160 --0.5860 -0.3885 3.8563 --0.6500 -0.4500 3.8965 -primID: 3 -startangle_c: 9 -endpose_c: -14 -3 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0741 -0.0305 3.5190 --0.1493 -0.0582 3.4718 --0.2256 -0.0824 3.4246 --0.3031 -0.1030 3.3775 --0.3814 -0.1199 3.3303 --0.4604 -0.1330 3.2831 --0.5400 -0.1424 3.2359 --0.6199 -0.1481 3.1888 --0.7000 -0.1500 3.1416 -primID: 4 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 5 -startangle_c: 9 -endpose_c: -6 -1 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0320 -0.0105 3.4967 --0.0644 -0.0197 3.4590 --0.0973 -0.0278 3.4214 --0.1304 -0.0346 3.3837 --0.1639 -0.0402 3.3461 --0.1977 -0.0446 3.3085 --0.2316 -0.0476 3.2708 --0.2658 -0.0495 3.2332 --0.3000 -0.0500 3.1955 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 7 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 8 -startangle_c: 9 -endpose_c: -7 -2 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0389 -0.0111 3.5343 --0.0778 -0.0222 3.5343 --0.1167 -0.0333 3.5343 --0.1556 -0.0444 3.5343 --0.1944 -0.0556 3.5343 --0.2333 -0.0667 3.5343 --0.2722 -0.0778 3.5343 --0.3111 -0.0889 3.5343 --0.3500 -0.1000 3.5343 -primID: 9 -startangle_c: 9 -endpose_c: -5 -4 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0278 -0.0222 3.5343 --0.0556 -0.0444 3.5343 --0.0833 -0.0667 3.5343 --0.1111 -0.0889 3.5343 --0.1389 -0.1111 3.5343 --0.1667 -0.1333 3.5343 --0.1944 -0.1556 3.5343 --0.2222 -0.1778 3.5343 --0.2500 -0.2000 3.5343 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: -11 -14 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0705 -0.0705 3.9270 --0.1411 -0.1411 3.9270 --0.2116 -0.2116 3.9270 --0.2816 -0.2828 3.9617 --0.3469 -0.3581 4.0333 --0.4068 -0.4379 4.1049 --0.4607 -0.5218 4.1765 --0.5086 -0.6093 4.2481 --0.5500 -0.7000 4.3197 -primID: 3 -startangle_c: 10 -endpose_c: -14 -11 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0705 -0.0705 3.9270 --0.1411 -0.1411 3.9270 --0.2116 -0.2116 3.9270 --0.2828 -0.2816 3.8923 --0.3581 -0.3469 3.8207 --0.4379 -0.4068 3.7491 --0.5218 -0.4607 3.6775 --0.6093 -0.5086 3.6059 --0.7000 -0.5500 3.5343 -primID: 4 -startangle_c: 10 -endpose_c: -4 -6 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0278 -0.0292 3.9704 --0.0543 -0.0595 4.0138 --0.0795 -0.0910 4.0572 --0.1033 -0.1235 4.1006 --0.1257 -0.1571 4.1440 --0.1466 -0.1916 4.1874 --0.1659 -0.2269 4.2308 --0.1838 -0.2631 4.2742 --0.2000 -0.3000 4.3176 -primID: 5 -startangle_c: 10 -endpose_c: -6 -4 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0292 -0.0278 3.8836 --0.0595 -0.0543 3.8402 --0.0910 -0.0795 3.7968 --0.1235 -0.1033 3.7534 --0.1571 -0.1257 3.7100 --0.1916 -0.1466 3.6666 --0.2269 -0.1659 3.6232 --0.2631 -0.1838 3.5798 --0.3000 -0.2000 3.5364 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 7 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 8 -startangle_c: 10 -endpose_c: -6 -7 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0389 3.9270 --0.0667 -0.0778 3.9270 --0.1000 -0.1167 3.9270 --0.1333 -0.1556 3.9270 --0.1667 -0.1944 3.9270 --0.2000 -0.2333 3.9270 --0.2333 -0.2722 3.9270 --0.2667 -0.3111 3.9270 --0.3000 -0.3500 3.9270 -primID: 9 -startangle_c: 10 -endpose_c: -7 -6 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0389 -0.0333 3.9270 --0.0778 -0.0667 3.9270 --0.1167 -0.1000 3.9270 --0.1556 -0.1333 3.9270 --0.1944 -0.1667 3.9270 --0.2333 -0.2000 3.9270 --0.2722 -0.2333 3.9270 --0.3111 -0.2667 3.9270 --0.3500 -0.3000 3.9270 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: -9 -13 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0378 -0.0793 4.2794 --0.0787 -0.1572 4.2392 --0.1229 -0.2334 4.1989 --0.1701 -0.3079 4.1587 --0.2204 -0.3805 4.1185 --0.2736 -0.4512 4.0782 --0.3296 -0.5197 4.0380 --0.3885 -0.5860 3.9977 --0.4500 -0.6500 3.9575 -primID: 3 -startangle_c: 11 -endpose_c: -3 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0305 -0.0741 4.3350 --0.0582 -0.1493 4.3822 --0.0824 -0.2256 4.4294 --0.1030 -0.3031 4.4765 --0.1199 -0.3814 4.5237 --0.1330 -0.4604 4.5709 --0.1424 -0.5400 4.6180 --0.1481 -0.6199 4.6652 --0.1500 -0.7000 4.7124 -primID: 4 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 5 -startangle_c: 11 -endpose_c: -1 -6 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0105 -0.0320 4.3573 --0.0197 -0.0644 4.3950 --0.0278 -0.0973 4.4326 --0.0346 -0.1304 4.4702 --0.0402 -0.1639 4.5079 --0.0446 -0.1977 4.5455 --0.0476 -0.2316 4.5832 --0.0495 -0.2658 4.6208 --0.0500 -0.3000 4.6584 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 7 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 8 -startangle_c: 11 -endpose_c: -2 -7 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0389 4.3197 --0.0222 -0.0778 4.3197 --0.0333 -0.1167 4.3197 --0.0444 -0.1556 4.3197 --0.0556 -0.1944 4.3197 --0.0667 -0.2333 4.3197 --0.0778 -0.2722 4.3197 --0.0889 -0.3111 4.3197 --0.1000 -0.3500 4.3197 -primID: 9 -startangle_c: 11 -endpose_c: -4 -5 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0222 -0.0278 4.3197 --0.0444 -0.0556 4.3197 --0.0667 -0.0833 4.3197 --0.0889 -0.1111 4.3197 --0.1111 -0.1389 4.3197 --0.1333 -0.1667 4.3197 --0.1556 -0.1944 4.3197 --0.1778 -0.2222 4.3197 --0.2000 -0.2500 4.3197 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 4 -16 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0095 -0.0904 4.7473 -0.0222 -0.1806 4.7823 -0.0381 -0.2707 4.8172 -0.0572 -0.3604 4.8522 -0.0795 -0.4496 4.8871 -0.1050 -0.5383 4.9221 -0.1335 -0.6264 4.9570 -0.1652 -0.7136 4.9919 -0.2000 -0.8000 5.0269 -primID: 3 -startangle_c: 12 -endpose_c: -4 -16 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0095 -0.0904 4.6774 --0.0222 -0.1806 4.6425 --0.0381 -0.2707 4.6076 --0.0572 -0.3604 4.5726 --0.0795 -0.4496 4.5377 --0.1050 -0.5383 4.5027 --0.1335 -0.6264 4.4678 --0.1652 -0.7136 4.4328 --0.2000 -0.8000 4.3979 -primID: 4 -startangle_c: 12 -endpose_c: 2 -5 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0085 -0.0280 4.7346 -0.0177 -0.0559 4.7569 -0.0275 -0.0839 4.7792 -0.0380 -0.1117 4.8014 -0.0491 -0.1396 4.8237 -0.0609 -0.1673 4.8459 -0.0733 -0.1950 4.8682 -0.0863 -0.2225 4.8904 -0.1000 -0.2500 4.9127 -primID: 5 -startangle_c: 12 -endpose_c: -2 -5 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0085 -0.0280 4.6901 --0.0177 -0.0559 4.6679 --0.0275 -0.0839 4.6456 --0.0380 -0.1117 4.6234 --0.0491 -0.1396 4.6011 --0.0609 -0.1673 4.5788 --0.0733 -0.1950 4.5566 --0.0863 -0.2225 4.5343 --0.1000 -0.2500 4.5121 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 7 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 8 -startangle_c: 12 -endpose_c: 1 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0056 -0.0444 4.7124 -0.0111 -0.0889 4.7124 -0.0167 -0.1333 4.7124 -0.0222 -0.1778 4.7124 -0.0278 -0.2222 4.7124 -0.0333 -0.2667 4.7124 -0.0389 -0.3111 4.7124 -0.0444 -0.3556 4.7124 -0.0500 -0.4000 4.7124 -primID: 9 -startangle_c: 12 -endpose_c: -1 -8 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0056 -0.0444 4.7124 --0.0111 -0.0889 4.7124 --0.0167 -0.1333 4.7124 --0.0222 -0.1778 4.7124 --0.0278 -0.2222 4.7124 --0.0333 -0.2667 4.7124 --0.0389 -0.3111 4.7124 --0.0444 -0.3556 4.7124 --0.0500 -0.4000 4.7124 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: 9 -13 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0378 -0.0793 5.1453 -0.0787 -0.1572 5.1856 -0.1229 -0.2334 5.2258 -0.1701 -0.3079 5.2661 -0.2204 -0.3805 5.3063 -0.2736 -0.4512 5.3466 -0.3296 -0.5197 5.3868 -0.3885 -0.5860 5.4271 -0.4500 -0.6500 5.4673 -primID: 3 -startangle_c: 13 -endpose_c: 3 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0305 -0.0741 5.0898 -0.0582 -0.1493 5.0426 -0.0824 -0.2256 4.9954 -0.1030 -0.3031 4.9482 -0.1199 -0.3814 4.9011 -0.1330 -0.4604 4.8539 -0.1424 -0.5400 4.8067 -0.1481 -0.6199 4.7596 -0.1500 -0.7000 4.7124 -primID: 4 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 5 -startangle_c: 13 -endpose_c: 1 -6 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0105 -0.0320 5.0674 -0.0197 -0.0644 5.0298 -0.0278 -0.0973 4.9922 -0.0346 -0.1304 4.9545 -0.0402 -0.1639 4.9169 -0.0446 -0.1977 4.8793 -0.0476 -0.2316 4.8416 -0.0495 -0.2658 4.8040 -0.0500 -0.3000 4.7663 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 7 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 8 -startangle_c: 13 -endpose_c: 2 -7 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0389 5.1051 -0.0222 -0.0778 5.1051 -0.0333 -0.1167 5.1051 -0.0444 -0.1556 5.1051 -0.0556 -0.1944 5.1051 -0.0667 -0.2333 5.1051 -0.0778 -0.2722 5.1051 -0.0889 -0.3111 5.1051 -0.1000 -0.3500 5.1051 -primID: 9 -startangle_c: 13 -endpose_c: 4 -5 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0222 -0.0278 5.1051 -0.0444 -0.0556 5.1051 -0.0667 -0.0833 5.1051 -0.0889 -0.1111 5.1051 -0.1111 -0.1389 5.1051 -0.1333 -0.1667 5.1051 -0.1556 -0.1944 5.1051 -0.1778 -0.2222 5.1051 -0.2000 -0.2500 5.1051 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: 14 -11 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0705 -0.0705 5.4978 -0.1411 -0.1411 5.4978 -0.2116 -0.2116 5.4978 -0.2828 -0.2816 5.5325 -0.3581 -0.3469 5.6041 -0.4379 -0.4068 5.6757 -0.5218 -0.4607 5.7473 -0.6093 -0.5086 5.8189 -0.7000 -0.5500 5.8905 -primID: 3 -startangle_c: 14 -endpose_c: 11 -14 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0705 -0.0705 5.4978 -0.1411 -0.1411 5.4978 -0.2116 -0.2116 5.4978 -0.2816 -0.2828 5.4631 -0.3469 -0.3581 5.3915 -0.4068 -0.4379 5.3199 -0.4607 -0.5218 5.2483 -0.5086 -0.6093 5.1767 -0.5500 -0.7000 5.1051 -primID: 4 -startangle_c: 14 -endpose_c: 6 -4 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0292 -0.0278 5.5412 -0.0595 -0.0543 5.5846 -0.0910 -0.0795 5.6280 -0.1235 -0.1033 5.6714 -0.1571 -0.1257 5.7148 -0.1916 -0.1466 5.7582 -0.2269 -0.1659 5.8016 -0.2631 -0.1838 5.8450 -0.3000 -0.2000 5.8884 -primID: 5 -startangle_c: 14 -endpose_c: 4 -6 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0278 -0.0292 5.4544 -0.0543 -0.0595 5.4110 -0.0795 -0.0910 5.3676 -0.1033 -0.1235 5.3242 -0.1257 -0.1571 5.2808 -0.1466 -0.1916 5.2374 -0.1659 -0.2269 5.1940 -0.1838 -0.2631 5.1506 -0.2000 -0.3000 5.1072 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 7 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 8 -startangle_c: 14 -endpose_c: 7 -6 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0389 -0.0333 5.4978 -0.0778 -0.0667 5.4978 -0.1167 -0.1000 5.4978 -0.1556 -0.1333 5.4978 -0.1944 -0.1667 5.4978 -0.2333 -0.2000 5.4978 -0.2722 -0.2333 5.4978 -0.3111 -0.2667 5.4978 -0.3500 -0.3000 5.4978 -primID: 9 -startangle_c: 14 -endpose_c: 6 -7 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0389 5.4978 -0.0667 -0.0778 5.4978 -0.1000 -0.1167 5.4978 -0.1333 -0.1556 5.4978 -0.1667 -0.1944 5.4978 -0.2000 -0.2333 5.4978 -0.2333 -0.2722 5.4978 -0.2667 -0.3111 5.4978 -0.3000 -0.3500 5.4978 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: 13 -9 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0793 -0.0378 5.8502 -0.1572 -0.0787 5.8100 -0.2334 -0.1229 5.7697 -0.3079 -0.1701 5.7295 -0.3805 -0.2204 5.6892 -0.4512 -0.2736 5.6490 -0.5197 -0.3296 5.6088 -0.5860 -0.3885 5.5685 -0.6500 -0.4500 5.5283 -primID: 3 -startangle_c: 15 -endpose_c: 14 -3 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0741 -0.0305 5.9058 -0.1493 -0.0582 5.9530 -0.2256 -0.0824 6.0002 -0.3031 -0.1030 6.0473 -0.3814 -0.1199 6.0945 -0.4604 -0.1330 6.1417 -0.5400 -0.1424 6.1888 -0.6199 -0.1481 6.2360 -0.7000 -0.1500 6.2832 -primID: 4 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 5 -startangle_c: 15 -endpose_c: 6 -1 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0320 -0.0105 5.9281 -0.0644 -0.0197 5.9658 -0.0973 -0.0278 6.0034 -0.1304 -0.0346 6.0410 -0.1639 -0.0402 6.0787 -0.1977 -0.0446 6.1163 -0.2316 -0.0476 6.1540 -0.2658 -0.0495 6.1916 -0.3000 -0.0500 6.2292 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 7 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 1000 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 -primID: 8 -startangle_c: 15 -endpose_c: 7 -2 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0389 -0.0111 5.8905 -0.0778 -0.0222 5.8905 -0.1167 -0.0333 5.8905 -0.1556 -0.0444 5.8905 -0.1944 -0.0556 5.8905 -0.2333 -0.0667 5.8905 -0.2722 -0.0778 5.8905 -0.3111 -0.0889 5.8905 -0.3500 -0.1000 5.8905 -primID: 9 -startangle_c: 15 -endpose_c: 5 -4 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0278 -0.0222 5.8905 -0.0556 -0.0444 5.8905 -0.0833 -0.0667 5.8905 -0.1111 -0.0889 5.8905 -0.1389 -0.1111 5.8905 -0.1667 -0.1333 5.8905 -0.1944 -0.1556 5.8905 -0.2222 -0.1778 5.8905 -0.2500 -0.2000 5.8905 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim deleted file mode 100755 index de16976..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.100000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0556 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.0778 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1000 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 4 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0556 0.0000 0.0000 --0.0667 0.0000 0.0000 --0.0778 0.0000 0.0000 --0.0889 0.0000 0.0000 --0.1000 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 4 1 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0048 0.0349 -0.0903 0.0111 0.0699 -0.1353 0.0191 0.1048 -0.1802 0.0286 0.1398 -0.2248 0.0398 0.1747 -0.2692 0.0525 0.2097 -0.3132 0.0668 0.2446 -0.3568 0.0826 0.2796 -0.4000 0.1000 0.3145 -primID: 4 -startangle_c: 0 -endpose_c: 4 -1 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0048 -0.0349 -0.0903 -0.0111 -0.0699 -0.1353 -0.0191 -0.1048 -0.1802 -0.0286 -0.1398 -0.2248 -0.0398 -0.1747 -0.2692 -0.0525 -0.2097 -0.3132 -0.0668 -0.2446 -0.3568 -0.0826 -0.2796 -0.4000 -0.1000 -0.3145 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0222 0.0111 0.3927 -0.0444 0.0222 0.3927 -0.0667 0.0333 0.3927 -0.0889 0.0444 0.3927 -0.1111 0.0556 0.3927 -0.1333 0.0667 0.3927 -0.1556 0.0778 0.3927 -0.1778 0.0889 0.3927 -0.2000 0.1000 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0667 0.0333 0.3927 -0.1333 0.0667 0.3927 -0.2000 0.1000 0.3927 -0.2667 0.1333 0.3927 -0.3333 0.1667 0.3927 -0.4000 0.2000 0.3927 -0.4667 0.2333 0.3927 -0.5333 0.2667 0.3927 -0.6000 0.3000 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0222 -0.0111 0.3927 --0.0444 -0.0222 0.3927 --0.0667 -0.0333 0.3927 --0.0889 -0.0444 0.3927 --0.1111 -0.0556 0.3927 --0.1333 -0.0667 0.3927 --0.1556 -0.0778 0.3927 --0.1778 -0.0889 0.3927 --0.2000 -0.1000 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 3 2 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0369 0.0162 0.4345 -0.0731 0.0339 0.4783 -0.1085 0.0533 0.5222 -0.1430 0.0741 0.5661 -0.1766 0.0965 0.6099 -0.2091 0.1203 0.6538 -0.2405 0.1455 0.6977 -0.2709 0.1721 0.7415 -0.3000 0.2000 0.7854 -primID: 4 -startangle_c: 1 -endpose_c: 4 1 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0427 0.0177 0.3927 -0.0854 0.0354 0.3927 -0.1283 0.0523 0.3477 -0.1722 0.0668 0.2898 -0.2168 0.0787 0.2318 -0.2621 0.0880 0.1739 -0.3078 0.0947 0.1159 -0.3538 0.0987 0.0580 -0.4000 0.1000 0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0111 0.0111 0.7854 -0.0222 0.0222 0.7854 -0.0333 0.0333 0.7854 -0.0444 0.0444 0.7854 -0.0556 0.0556 0.7854 -0.0667 0.0667 0.7854 -0.0778 0.0778 0.7854 -0.0889 0.0889 0.7854 -0.1000 0.1000 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 3 3 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0111 -0.0111 0.7854 --0.0222 -0.0222 0.7854 --0.0333 -0.0333 0.7854 --0.0444 -0.0444 0.7854 --0.0556 -0.0556 0.7854 --0.0667 -0.0667 0.7854 --0.0778 -0.0778 0.7854 --0.0889 -0.0889 0.7854 --0.1000 -0.1000 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 2 4 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0262 0.0411 0.8119 -0.0515 0.0831 0.8384 -0.0758 0.1260 0.8649 -0.0991 0.1697 0.8913 -0.1214 0.2142 0.9178 -0.1426 0.2595 0.9443 -0.1628 0.3056 0.9708 -0.1820 0.3525 0.9973 -0.2000 0.4000 1.0238 -primID: 4 -startangle_c: 2 -endpose_c: 4 2 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0411 0.0262 0.7589 -0.0831 0.0515 0.7324 -0.1260 0.0758 0.7059 -0.1697 0.0991 0.6795 -0.2142 0.1214 0.6530 -0.2595 0.1426 0.6265 -0.3056 0.1628 0.6000 -0.3525 0.1820 0.5735 -0.4000 0.2000 0.5470 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0222 1.1781 -0.0222 0.0444 1.1781 -0.0333 0.0667 1.1781 -0.0444 0.0889 1.1781 -0.0556 0.1111 1.1781 -0.0667 0.1333 1.1781 -0.0778 0.1556 1.1781 -0.0889 0.1778 1.1781 -0.1000 0.2000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0333 0.0667 1.1781 -0.0667 0.1333 1.1781 -0.1000 0.2000 1.1781 -0.1333 0.2667 1.1781 -0.1667 0.3333 1.1781 -0.2000 0.4000 1.1781 -0.2333 0.4667 1.1781 -0.2667 0.5333 1.1781 -0.3000 0.6000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0111 -0.0222 1.1781 --0.0222 -0.0444 1.1781 --0.0333 -0.0667 1.1781 --0.0444 -0.0889 1.1781 --0.0556 -0.1111 1.1781 --0.0667 -0.1333 1.1781 --0.0778 -0.1556 1.1781 --0.0889 -0.1778 1.1781 --0.1000 -0.2000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 2 3 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0162 0.0369 1.1363 -0.0339 0.0731 1.0925 -0.0533 0.1085 1.0486 -0.0741 0.1430 1.0047 -0.0965 0.1766 0.9609 -0.1203 0.2091 0.9170 -0.1455 0.2405 0.8731 -0.1721 0.2709 0.8293 -0.2000 0.3000 0.7854 -primID: 4 -startangle_c: 3 -endpose_c: 1 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0177 0.0427 1.1781 -0.0354 0.0854 1.1781 -0.0523 0.1283 1.2231 -0.0668 0.1722 1.2810 -0.0787 0.2168 1.3390 -0.0880 0.2621 1.3969 -0.0947 0.3078 1.4549 -0.0987 0.3538 1.5128 -0.1000 0.4000 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0556 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.0778 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1000 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0556 1.5708 -0.0000 -0.0667 1.5708 -0.0000 -0.0778 1.5708 -0.0000 -0.0889 1.5708 -0.0000 -0.1000 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 4 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0048 0.0452 1.6057 --0.0111 0.0903 1.6407 --0.0191 0.1353 1.6756 --0.0286 0.1802 1.7106 --0.0398 0.2248 1.7455 --0.0525 0.2692 1.7805 --0.0668 0.3132 1.8154 --0.0826 0.3568 1.8503 --0.1000 0.4000 1.8853 -primID: 4 -startangle_c: 4 -endpose_c: 1 4 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0048 0.0452 1.5359 -0.0111 0.0903 1.5009 -0.0191 0.1353 1.4660 -0.0286 0.1802 1.4310 -0.0398 0.2248 1.3961 -0.0525 0.2692 1.3611 -0.0668 0.3132 1.3262 -0.0826 0.3568 1.2912 -0.1000 0.4000 1.2563 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0222 1.9635 --0.0222 0.0444 1.9635 --0.0333 0.0667 1.9635 --0.0444 0.0889 1.9635 --0.0556 0.1111 1.9635 --0.0667 0.1333 1.9635 --0.0778 0.1556 1.9635 --0.0889 0.1778 1.9635 --0.1000 0.2000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0333 0.0667 1.9635 --0.0667 0.1333 1.9635 --0.1000 0.2000 1.9635 --0.1333 0.2667 1.9635 --0.1667 0.3333 1.9635 --0.2000 0.4000 1.9635 --0.2333 0.4667 1.9635 --0.2667 0.5333 1.9635 --0.3000 0.6000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0111 -0.0222 1.9635 -0.0222 -0.0444 1.9635 -0.0333 -0.0667 1.9635 -0.0444 -0.0889 1.9635 -0.0556 -0.1111 1.9635 -0.0667 -0.1333 1.9635 -0.0778 -0.1556 1.9635 -0.0889 -0.1778 1.9635 -0.1000 -0.2000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -2 3 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0162 0.0369 2.0053 --0.0339 0.0731 2.0491 --0.0533 0.1085 2.0930 --0.0741 0.1430 2.1369 --0.0965 0.1766 2.1807 --0.1203 0.2091 2.2246 --0.1455 0.2405 2.2685 --0.1721 0.2709 2.3123 --0.2000 0.3000 2.3562 -primID: 4 -startangle_c: 5 -endpose_c: -1 4 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0177 0.0427 1.9635 --0.0354 0.0854 1.9635 --0.0523 0.1283 1.9185 --0.0668 0.1722 1.8606 --0.0787 0.2168 1.8026 --0.0880 0.2621 1.7447 --0.0947 0.3078 1.6867 --0.0987 0.3538 1.6287 --0.1000 0.4000 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0111 0.0111 2.3562 --0.0222 0.0222 2.3562 --0.0333 0.0333 2.3562 --0.0444 0.0444 2.3562 --0.0556 0.0556 2.3562 --0.0667 0.0667 2.3562 --0.0778 0.0778 2.3562 --0.0889 0.0889 2.3562 --0.1000 0.1000 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -3 3 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0111 -0.0111 2.3562 -0.0222 -0.0222 2.3562 -0.0333 -0.0333 2.3562 -0.0444 -0.0444 2.3562 -0.0556 -0.0556 2.3562 -0.0667 -0.0667 2.3562 -0.0778 -0.0778 2.3562 -0.0889 -0.0889 2.3562 -0.1000 -0.1000 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -4 2 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0411 0.0262 2.3827 --0.0831 0.0515 2.4092 --0.1260 0.0758 2.4357 --0.1697 0.0991 2.4621 --0.2142 0.1214 2.4886 --0.2595 0.1426 2.5151 --0.3056 0.1628 2.5416 --0.3525 0.1820 2.5681 --0.4000 0.2000 2.5946 -primID: 4 -startangle_c: 6 -endpose_c: -2 4 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0262 0.0411 2.3297 --0.0515 0.0831 2.3032 --0.0758 0.1260 2.2767 --0.0991 0.1697 2.2502 --0.1214 0.2142 2.2238 --0.1426 0.2595 2.1973 --0.1628 0.3056 2.1708 --0.1820 0.3525 2.1443 --0.2000 0.4000 2.1178 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0222 0.0111 2.7489 --0.0444 0.0222 2.7489 --0.0667 0.0333 2.7489 --0.0889 0.0444 2.7489 --0.1111 0.0556 2.7489 --0.1333 0.0667 2.7489 --0.1556 0.0778 2.7489 --0.1778 0.0889 2.7489 --0.2000 0.1000 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0667 0.0333 2.7489 --0.1333 0.0667 2.7489 --0.2000 0.1000 2.7489 --0.2667 0.1333 2.7489 --0.3333 0.1667 2.7489 --0.4000 0.2000 2.7489 --0.4667 0.2333 2.7489 --0.5333 0.2667 2.7489 --0.6000 0.3000 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0222 -0.0111 2.7489 -0.0444 -0.0222 2.7489 -0.0667 -0.0333 2.7489 -0.0889 -0.0444 2.7489 -0.1111 -0.0556 2.7489 -0.1333 -0.0667 2.7489 -0.1556 -0.0778 2.7489 -0.1778 -0.0889 2.7489 -0.2000 -0.1000 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -3 2 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0369 0.0162 2.7071 --0.0731 0.0339 2.6633 --0.1085 0.0533 2.6194 --0.1430 0.0741 2.5755 --0.1766 0.0965 2.5317 --0.2091 0.1203 2.4878 --0.2405 0.1455 2.4439 --0.2709 0.1721 2.4001 --0.3000 0.2000 2.3562 -primID: 4 -startangle_c: 7 -endpose_c: -4 1 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0427 0.0177 2.7489 --0.0854 0.0354 2.7489 --0.1283 0.0523 2.7939 --0.1722 0.0668 2.8518 --0.2168 0.0787 2.9098 --0.2621 0.0880 2.9677 --0.3078 0.0947 3.0257 --0.3538 0.0987 3.0836 --0.4000 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0556 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.0778 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1000 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -4 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0556 0.0000 3.1416 -0.0667 0.0000 3.1416 -0.0778 0.0000 3.1416 -0.0889 0.0000 3.1416 -0.1000 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -4 -1 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 -0.0048 3.1765 --0.0903 -0.0111 3.2115 --0.1353 -0.0191 3.2464 --0.1802 -0.0286 3.2814 --0.2248 -0.0398 3.3163 --0.2692 -0.0525 3.3513 --0.3132 -0.0668 3.3862 --0.3568 -0.0826 3.4211 --0.4000 -0.1000 3.4561 -primID: 4 -startangle_c: 8 -endpose_c: -4 1 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0048 3.1066 --0.0903 0.0111 3.0717 --0.1353 0.0191 3.0368 --0.1802 0.0286 3.0018 --0.2248 0.0398 2.9669 --0.2692 0.0525 2.9319 --0.3132 0.0668 2.8970 --0.3568 0.0826 2.8620 --0.4000 0.1000 2.8271 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0222 -0.0111 3.5343 --0.0444 -0.0222 3.5343 --0.0667 -0.0333 3.5343 --0.0889 -0.0444 3.5343 --0.1111 -0.0556 3.5343 --0.1333 -0.0667 3.5343 --0.1556 -0.0778 3.5343 --0.1778 -0.0889 3.5343 --0.2000 -0.1000 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0667 -0.0333 3.5343 --0.1333 -0.0667 3.5343 --0.2000 -0.1000 3.5343 --0.2667 -0.1333 3.5343 --0.3333 -0.1667 3.5343 --0.4000 -0.2000 3.5343 --0.4667 -0.2333 3.5343 --0.5333 -0.2667 3.5343 --0.6000 -0.3000 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0222 0.0111 3.5343 -0.0444 0.0222 3.5343 -0.0667 0.0333 3.5343 -0.0889 0.0444 3.5343 -0.1111 0.0556 3.5343 -0.1333 0.0667 3.5343 -0.1556 0.0778 3.5343 -0.1778 0.0889 3.5343 -0.2000 0.1000 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -3 -2 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0369 -0.0162 3.5761 --0.0731 -0.0339 3.6199 --0.1085 -0.0533 3.6638 --0.1430 -0.0741 3.7077 --0.1766 -0.0965 3.7515 --0.2091 -0.1203 3.7954 --0.2405 -0.1455 3.8393 --0.2709 -0.1721 3.8831 --0.3000 -0.2000 3.9270 -primID: 4 -startangle_c: 9 -endpose_c: -4 -1 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0427 -0.0177 3.5343 --0.0854 -0.0354 3.5343 --0.1283 -0.0523 3.4893 --0.1722 -0.0668 3.4313 --0.2168 -0.0787 3.3734 --0.2621 -0.0880 3.3154 --0.3078 -0.0947 3.2575 --0.3538 -0.0987 3.1995 --0.4000 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0111 -0.0111 3.9270 --0.0222 -0.0222 3.9270 --0.0333 -0.0333 3.9270 --0.0444 -0.0444 3.9270 --0.0556 -0.0556 3.9270 --0.0667 -0.0667 3.9270 --0.0778 -0.0778 3.9270 --0.0889 -0.0889 3.9270 --0.1000 -0.1000 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -3 -3 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0111 0.0111 3.9270 -0.0222 0.0222 3.9270 -0.0333 0.0333 3.9270 -0.0444 0.0444 3.9270 -0.0556 0.0556 3.9270 -0.0667 0.0667 3.9270 -0.0778 0.0778 3.9270 -0.0889 0.0889 3.9270 -0.1000 0.1000 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -2 -4 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0262 -0.0411 3.9535 --0.0515 -0.0831 3.9800 --0.0758 -0.1260 4.0064 --0.0991 -0.1697 4.0329 --0.1214 -0.2142 4.0594 --0.1426 -0.2595 4.0859 --0.1628 -0.3056 4.1124 --0.1820 -0.3525 4.1389 --0.2000 -0.4000 4.1654 -primID: 4 -startangle_c: 10 -endpose_c: -4 -2 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0411 -0.0262 3.9005 --0.0831 -0.0515 3.8740 --0.1260 -0.0758 3.8475 --0.1697 -0.0991 3.8210 --0.2142 -0.1214 3.7946 --0.2595 -0.1426 3.7681 --0.3056 -0.1628 3.7416 --0.3525 -0.1820 3.7151 --0.4000 -0.2000 3.6886 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0222 4.3197 --0.0222 -0.0444 4.3197 --0.0333 -0.0667 4.3197 --0.0444 -0.0889 4.3197 --0.0556 -0.1111 4.3197 --0.0667 -0.1333 4.3197 --0.0778 -0.1556 4.3197 --0.0889 -0.1778 4.3197 --0.1000 -0.2000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0333 -0.0667 4.3197 --0.0667 -0.1333 4.3197 --0.1000 -0.2000 4.3197 --0.1333 -0.2667 4.3197 --0.1667 -0.3333 4.3197 --0.2000 -0.4000 4.3197 --0.2333 -0.4667 4.3197 --0.2667 -0.5333 4.3197 --0.3000 -0.6000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0111 0.0222 4.3197 -0.0222 0.0444 4.3197 -0.0333 0.0667 4.3197 -0.0444 0.0889 4.3197 -0.0556 0.1111 4.3197 -0.0667 0.1333 4.3197 -0.0778 0.1556 4.3197 -0.0889 0.1778 4.3197 -0.1000 0.2000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -2 -3 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0162 -0.0369 4.2779 --0.0339 -0.0731 4.2341 --0.0533 -0.1085 4.1902 --0.0741 -0.1430 4.1463 --0.0965 -0.1766 4.1025 --0.1203 -0.2091 4.0586 --0.1455 -0.2405 4.0147 --0.1721 -0.2709 3.9709 --0.2000 -0.3000 3.9270 -primID: 4 -startangle_c: 11 -endpose_c: -1 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0177 -0.0427 4.3197 --0.0354 -0.0854 4.3197 --0.0523 -0.1283 4.3647 --0.0668 -0.1722 4.4226 --0.0787 -0.2168 4.4806 --0.0880 -0.2621 4.5385 --0.0947 -0.3078 4.5965 --0.0987 -0.3538 4.6544 --0.1000 -0.4000 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0556 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.0778 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1000 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0556 4.7124 -0.0000 0.0667 4.7124 -0.0000 0.0778 4.7124 -0.0000 0.0889 4.7124 -0.0000 0.1000 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -4 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0048 -0.0452 4.7473 -0.0111 -0.0903 4.7823 -0.0191 -0.1353 4.8172 -0.0286 -0.1802 4.8522 -0.0398 -0.2248 4.8871 -0.0525 -0.2692 4.9221 -0.0668 -0.3132 4.9570 -0.0826 -0.3568 4.9919 -0.1000 -0.4000 5.0269 -primID: 4 -startangle_c: 12 -endpose_c: -1 -4 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0048 -0.0452 4.6774 --0.0111 -0.0903 4.6425 --0.0191 -0.1353 4.6076 --0.0286 -0.1802 4.5726 --0.0398 -0.2248 4.5377 --0.0525 -0.2692 4.5027 --0.0668 -0.3132 4.4678 --0.0826 -0.3568 4.4328 --0.1000 -0.4000 4.3979 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0222 5.1051 -0.0222 -0.0444 5.1051 -0.0333 -0.0667 5.1051 -0.0444 -0.0889 5.1051 -0.0556 -0.1111 5.1051 -0.0667 -0.1333 5.1051 -0.0778 -0.1556 5.1051 -0.0889 -0.1778 5.1051 -0.1000 -0.2000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0333 -0.0667 5.1051 -0.0667 -0.1333 5.1051 -0.1000 -0.2000 5.1051 -0.1333 -0.2667 5.1051 -0.1667 -0.3333 5.1051 -0.2000 -0.4000 5.1051 -0.2333 -0.4667 5.1051 -0.2667 -0.5333 5.1051 -0.3000 -0.6000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0111 0.0222 5.1051 --0.0222 0.0444 5.1051 --0.0333 0.0667 5.1051 --0.0444 0.0889 5.1051 --0.0556 0.1111 5.1051 --0.0667 0.1333 5.1051 --0.0778 0.1556 5.1051 --0.0889 0.1778 5.1051 --0.1000 0.2000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 2 -3 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0162 -0.0369 5.1469 -0.0339 -0.0731 5.1907 -0.0533 -0.1085 5.2346 -0.0741 -0.1430 5.2785 -0.0965 -0.1766 5.3223 -0.1203 -0.2091 5.3662 -0.1455 -0.2405 5.4101 -0.1721 -0.2709 5.4539 -0.2000 -0.3000 5.4978 -primID: 4 -startangle_c: 13 -endpose_c: 1 -4 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0177 -0.0427 5.1051 -0.0354 -0.0854 5.1051 -0.0523 -0.1283 5.0601 -0.0668 -0.1722 5.0021 -0.0787 -0.2168 4.9442 -0.0880 -0.2621 4.8862 -0.0947 -0.3078 4.8283 -0.0987 -0.3538 4.7703 -0.1000 -0.4000 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0111 -0.0111 5.4978 -0.0222 -0.0222 5.4978 -0.0333 -0.0333 5.4978 -0.0444 -0.0444 5.4978 -0.0556 -0.0556 5.4978 -0.0667 -0.0667 5.4978 -0.0778 -0.0778 5.4978 -0.0889 -0.0889 5.4978 -0.1000 -0.1000 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 3 -3 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0111 0.0111 5.4978 --0.0222 0.0222 5.4978 --0.0333 0.0333 5.4978 --0.0444 0.0444 5.4978 --0.0556 0.0556 5.4978 --0.0667 0.0667 5.4978 --0.0778 0.0778 5.4978 --0.0889 0.0889 5.4978 --0.1000 0.1000 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 4 -2 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0411 -0.0262 5.5243 -0.0831 -0.0515 5.5508 -0.1260 -0.0758 5.5772 -0.1697 -0.0991 5.6037 -0.2142 -0.1214 5.6302 -0.2595 -0.1426 5.6567 -0.3056 -0.1628 5.6832 -0.3525 -0.1820 5.7097 -0.4000 -0.2000 5.7362 -primID: 4 -startangle_c: 14 -endpose_c: 2 -4 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0262 -0.0411 5.4713 -0.0515 -0.0831 5.4448 -0.0758 -0.1260 5.4183 -0.0991 -0.1697 5.3918 -0.1214 -0.2142 5.3654 -0.1426 -0.2595 5.3389 -0.1628 -0.3056 5.3124 -0.1820 -0.3525 5.2859 -0.2000 -0.4000 5.2594 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0222 -0.0111 5.8905 -0.0444 -0.0222 5.8905 -0.0667 -0.0333 5.8905 -0.0889 -0.0444 5.8905 -0.1111 -0.0556 5.8905 -0.1333 -0.0667 5.8905 -0.1556 -0.0778 5.8905 -0.1778 -0.0889 5.8905 -0.2000 -0.1000 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0667 -0.0333 5.8905 -0.1333 -0.0667 5.8905 -0.2000 -0.1000 5.8905 -0.2667 -0.1333 5.8905 -0.3333 -0.1667 5.8905 -0.4000 -0.2000 5.8905 -0.4667 -0.2333 5.8905 -0.5333 -0.2667 5.8905 -0.6000 -0.3000 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0222 0.0111 5.8905 --0.0444 0.0222 5.8905 --0.0667 0.0333 5.8905 --0.0889 0.0444 5.8905 --0.1111 0.0556 5.8905 --0.1333 0.0667 5.8905 --0.1556 0.0778 5.8905 --0.1778 0.0889 5.8905 --0.2000 0.1000 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 3 -2 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0369 -0.0162 5.8487 -0.0731 -0.0339 5.8049 -0.1085 -0.0533 5.7610 -0.1430 -0.0741 5.7171 -0.1766 -0.0965 5.6733 -0.2091 -0.1203 5.6294 -0.2405 -0.1455 5.5855 -0.2709 -0.1721 5.5417 -0.3000 -0.2000 5.4978 -primID: 4 -startangle_c: 15 -endpose_c: 4 -1 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0427 -0.0177 5.8905 -0.0854 -0.0354 5.8905 -0.1283 -0.0523 5.9355 -0.1722 -0.0668 5.9934 -0.2168 -0.0787 6.0514 -0.2621 -0.0880 6.1093 -0.3078 -0.0947 6.1673 -0.3538 -0.0987 6.2252 -0.4000 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim deleted file mode 100755 index aa9670d..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.010000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0011 0.0000 0.0000 -0.0022 0.0000 0.0000 -0.0033 0.0000 0.0000 -0.0044 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0067 0.0000 0.0000 -0.0078 0.0000 0.0000 -0.0089 0.0000 0.0000 -0.0100 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 20 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1111 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1556 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0011 0.0000 0.0000 --0.0022 0.0000 0.0000 --0.0033 0.0000 0.0000 --0.0044 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0067 0.0000 0.0000 --0.0078 0.0000 0.0000 --0.0089 0.0000 0.0000 --0.0100 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 40 5 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 40 -5 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0022 0.0011 0.3927 -0.0044 0.0022 0.3927 -0.0067 0.0033 0.3927 -0.0089 0.0044 0.3927 -0.0111 0.0056 0.3927 -0.0133 0.0067 0.3927 -0.0156 0.0078 0.3927 -0.0178 0.0089 0.3927 -0.0200 0.0100 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 20 10 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0222 0.0111 0.3927 -0.0444 0.0222 0.3927 -0.0667 0.0333 0.3927 -0.0889 0.0444 0.3927 -0.1111 0.0556 0.3927 -0.1333 0.0667 0.3927 -0.1556 0.0778 0.3927 -0.1778 0.0889 0.3927 -0.2000 0.1000 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0022 -0.0011 0.3927 --0.0044 -0.0022 0.3927 --0.0067 -0.0033 0.3927 --0.0089 -0.0044 0.3927 --0.0111 -0.0056 0.3927 --0.0133 -0.0067 0.3927 --0.0156 -0.0078 0.3927 --0.0178 -0.0089 0.3927 --0.0200 -0.0100 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 25 20 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 35 10 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0011 0.0011 0.7854 -0.0022 0.0022 0.7854 -0.0033 0.0033 0.7854 -0.0044 0.0044 0.7854 -0.0056 0.0056 0.7854 -0.0067 0.0067 0.7854 -0.0078 0.0078 0.7854 -0.0089 0.0089 0.7854 -0.0100 0.0100 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 10 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0111 0.0111 0.7854 -0.0222 0.0222 0.7854 -0.0333 0.0333 0.7854 -0.0444 0.0444 0.7854 -0.0556 0.0556 0.7854 -0.0667 0.0667 0.7854 -0.0778 0.0778 0.7854 -0.0889 0.0889 0.7854 -0.1000 0.1000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0011 -0.0011 0.7854 --0.0022 -0.0022 0.7854 --0.0033 -0.0033 0.7854 --0.0044 -0.0044 0.7854 --0.0056 -0.0056 0.7854 --0.0067 -0.0067 0.7854 --0.0078 -0.0078 0.7854 --0.0089 -0.0089 0.7854 --0.0100 -0.0100 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 25 35 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 35 25 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0011 0.0022 1.1781 -0.0022 0.0044 1.1781 -0.0033 0.0067 1.1781 -0.0044 0.0089 1.1781 -0.0056 0.0111 1.1781 -0.0067 0.0133 1.1781 -0.0078 0.0156 1.1781 -0.0089 0.0178 1.1781 -0.0100 0.0200 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 10 20 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0111 0.0222 1.1781 -0.0222 0.0444 1.1781 -0.0333 0.0667 1.1781 -0.0444 0.0889 1.1781 -0.0556 0.1111 1.1781 -0.0667 0.1333 1.1781 -0.0778 0.1556 1.1781 -0.0889 0.1778 1.1781 -0.1000 0.2000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0011 -0.0022 1.1781 --0.0022 -0.0044 1.1781 --0.0033 -0.0067 1.1781 --0.0044 -0.0089 1.1781 --0.0056 -0.0111 1.1781 --0.0067 -0.0133 1.1781 --0.0078 -0.0156 1.1781 --0.0089 -0.0178 1.1781 --0.0100 -0.0200 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 20 25 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 10 35 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0011 1.5708 -0.0000 0.0022 1.5708 -0.0000 0.0033 1.5708 -0.0000 0.0044 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0067 1.5708 -0.0000 0.0078 1.5708 -0.0000 0.0089 1.5708 -0.0000 0.0100 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 20 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1111 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1556 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0011 1.5708 -0.0000 -0.0022 1.5708 -0.0000 -0.0033 1.5708 -0.0000 -0.0044 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0067 1.5708 -0.0000 -0.0078 1.5708 -0.0000 -0.0089 1.5708 -0.0000 -0.0100 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -5 40 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 5 40 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0011 0.0022 1.9635 --0.0022 0.0044 1.9635 --0.0033 0.0067 1.9635 --0.0044 0.0089 1.9635 --0.0056 0.0111 1.9635 --0.0067 0.0133 1.9635 --0.0078 0.0156 1.9635 --0.0089 0.0178 1.9635 --0.0100 0.0200 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -10 20 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0111 0.0222 1.9635 --0.0222 0.0444 1.9635 --0.0333 0.0667 1.9635 --0.0444 0.0889 1.9635 --0.0556 0.1111 1.9635 --0.0667 0.1333 1.9635 --0.0778 0.1556 1.9635 --0.0889 0.1778 1.9635 --0.1000 0.2000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0011 -0.0022 1.9635 -0.0022 -0.0044 1.9635 -0.0033 -0.0067 1.9635 -0.0044 -0.0089 1.9635 -0.0056 -0.0111 1.9635 -0.0067 -0.0133 1.9635 -0.0078 -0.0156 1.9635 -0.0089 -0.0178 1.9635 -0.0100 -0.0200 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -20 25 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -10 35 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0011 0.0011 2.3562 --0.0022 0.0022 2.3562 --0.0033 0.0033 2.3562 --0.0044 0.0044 2.3562 --0.0056 0.0056 2.3562 --0.0067 0.0067 2.3562 --0.0078 0.0078 2.3562 --0.0089 0.0089 2.3562 --0.0100 0.0100 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -10 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0111 0.0111 2.3562 --0.0222 0.0222 2.3562 --0.0333 0.0333 2.3562 --0.0444 0.0444 2.3562 --0.0556 0.0556 2.3562 --0.0667 0.0667 2.3562 --0.0778 0.0778 2.3562 --0.0889 0.0889 2.3562 --0.1000 0.1000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0011 -0.0011 2.3562 -0.0022 -0.0022 2.3562 -0.0033 -0.0033 2.3562 -0.0044 -0.0044 2.3562 -0.0056 -0.0056 2.3562 -0.0067 -0.0067 2.3562 -0.0078 -0.0078 2.3562 -0.0089 -0.0089 2.3562 -0.0100 -0.0100 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -35 25 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -25 35 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0022 0.0011 2.7489 --0.0044 0.0022 2.7489 --0.0067 0.0033 2.7489 --0.0089 0.0044 2.7489 --0.0111 0.0056 2.7489 --0.0133 0.0067 2.7489 --0.0156 0.0078 2.7489 --0.0178 0.0089 2.7489 --0.0200 0.0100 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -20 10 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0222 0.0111 2.7489 --0.0444 0.0222 2.7489 --0.0667 0.0333 2.7489 --0.0889 0.0444 2.7489 --0.1111 0.0556 2.7489 --0.1333 0.0667 2.7489 --0.1556 0.0778 2.7489 --0.1778 0.0889 2.7489 --0.2000 0.1000 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0022 -0.0011 2.7489 -0.0044 -0.0022 2.7489 -0.0067 -0.0033 2.7489 -0.0089 -0.0044 2.7489 -0.0111 -0.0056 2.7489 -0.0133 -0.0067 2.7489 -0.0156 -0.0078 2.7489 -0.0178 -0.0089 2.7489 -0.0200 -0.0100 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -25 20 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -35 10 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0011 0.0000 3.1416 --0.0022 0.0000 3.1416 --0.0033 0.0000 3.1416 --0.0044 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0067 0.0000 3.1416 --0.0078 0.0000 3.1416 --0.0089 0.0000 3.1416 --0.0100 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -20 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1111 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1556 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0011 0.0000 3.1416 -0.0022 0.0000 3.1416 -0.0033 0.0000 3.1416 -0.0044 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0067 0.0000 3.1416 -0.0078 0.0000 3.1416 -0.0089 0.0000 3.1416 -0.0100 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -40 -5 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -40 5 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0022 -0.0011 3.5343 --0.0044 -0.0022 3.5343 --0.0067 -0.0033 3.5343 --0.0089 -0.0044 3.5343 --0.0111 -0.0056 3.5343 --0.0133 -0.0067 3.5343 --0.0156 -0.0078 3.5343 --0.0178 -0.0089 3.5343 --0.0200 -0.0100 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -20 -10 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0222 -0.0111 3.5343 --0.0444 -0.0222 3.5343 --0.0667 -0.0333 3.5343 --0.0889 -0.0444 3.5343 --0.1111 -0.0556 3.5343 --0.1333 -0.0667 3.5343 --0.1556 -0.0778 3.5343 --0.1778 -0.0889 3.5343 --0.2000 -0.1000 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0022 0.0011 3.5343 -0.0044 0.0022 3.5343 -0.0067 0.0033 3.5343 -0.0089 0.0044 3.5343 -0.0111 0.0056 3.5343 -0.0133 0.0067 3.5343 -0.0156 0.0078 3.5343 -0.0178 0.0089 3.5343 -0.0200 0.0100 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -25 -20 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -35 -10 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0011 -0.0011 3.9270 --0.0022 -0.0022 3.9270 --0.0033 -0.0033 3.9270 --0.0044 -0.0044 3.9270 --0.0056 -0.0056 3.9270 --0.0067 -0.0067 3.9270 --0.0078 -0.0078 3.9270 --0.0089 -0.0089 3.9270 --0.0100 -0.0100 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -10 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0111 -0.0111 3.9270 --0.0222 -0.0222 3.9270 --0.0333 -0.0333 3.9270 --0.0444 -0.0444 3.9270 --0.0556 -0.0556 3.9270 --0.0667 -0.0667 3.9270 --0.0778 -0.0778 3.9270 --0.0889 -0.0889 3.9270 --0.1000 -0.1000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0011 0.0011 3.9270 -0.0022 0.0022 3.9270 -0.0033 0.0033 3.9270 -0.0044 0.0044 3.9270 -0.0056 0.0056 3.9270 -0.0067 0.0067 3.9270 -0.0078 0.0078 3.9270 -0.0089 0.0089 3.9270 -0.0100 0.0100 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -25 -35 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -35 -25 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0011 -0.0022 4.3197 --0.0022 -0.0044 4.3197 --0.0033 -0.0067 4.3197 --0.0044 -0.0089 4.3197 --0.0056 -0.0111 4.3197 --0.0067 -0.0133 4.3197 --0.0078 -0.0156 4.3197 --0.0089 -0.0178 4.3197 --0.0100 -0.0200 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -10 -20 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0111 -0.0222 4.3197 --0.0222 -0.0444 4.3197 --0.0333 -0.0667 4.3197 --0.0444 -0.0889 4.3197 --0.0556 -0.1111 4.3197 --0.0667 -0.1333 4.3197 --0.0778 -0.1556 4.3197 --0.0889 -0.1778 4.3197 --0.1000 -0.2000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0011 0.0022 4.3197 -0.0022 0.0044 4.3197 -0.0033 0.0067 4.3197 -0.0044 0.0089 4.3197 -0.0056 0.0111 4.3197 -0.0067 0.0133 4.3197 -0.0078 0.0156 4.3197 -0.0089 0.0178 4.3197 -0.0100 0.0200 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -20 -25 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -10 -35 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0011 4.7124 -0.0000 -0.0022 4.7124 -0.0000 -0.0033 4.7124 -0.0000 -0.0044 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0067 4.7124 -0.0000 -0.0078 4.7124 -0.0000 -0.0089 4.7124 -0.0000 -0.0100 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -20 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1111 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1556 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0011 4.7124 -0.0000 0.0022 4.7124 -0.0000 0.0033 4.7124 -0.0000 0.0044 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0067 4.7124 -0.0000 0.0078 4.7124 -0.0000 0.0089 4.7124 -0.0000 0.0100 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 5 -40 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -5 -40 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0011 -0.0022 5.1051 -0.0022 -0.0044 5.1051 -0.0033 -0.0067 5.1051 -0.0044 -0.0089 5.1051 -0.0056 -0.0111 5.1051 -0.0067 -0.0133 5.1051 -0.0078 -0.0156 5.1051 -0.0089 -0.0178 5.1051 -0.0100 -0.0200 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 10 -20 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0111 -0.0222 5.1051 -0.0222 -0.0444 5.1051 -0.0333 -0.0667 5.1051 -0.0444 -0.0889 5.1051 -0.0556 -0.1111 5.1051 -0.0667 -0.1333 5.1051 -0.0778 -0.1556 5.1051 -0.0889 -0.1778 5.1051 -0.1000 -0.2000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0011 0.0022 5.1051 --0.0022 0.0044 5.1051 --0.0033 0.0067 5.1051 --0.0044 0.0089 5.1051 --0.0056 0.0111 5.1051 --0.0067 0.0133 5.1051 --0.0078 0.0156 5.1051 --0.0089 0.0178 5.1051 --0.0100 0.0200 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 20 -25 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 10 -35 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0011 -0.0011 5.4978 -0.0022 -0.0022 5.4978 -0.0033 -0.0033 5.4978 -0.0044 -0.0044 5.4978 -0.0056 -0.0056 5.4978 -0.0067 -0.0067 5.4978 -0.0078 -0.0078 5.4978 -0.0089 -0.0089 5.4978 -0.0100 -0.0100 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 10 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0111 -0.0111 5.4978 -0.0222 -0.0222 5.4978 -0.0333 -0.0333 5.4978 -0.0444 -0.0444 5.4978 -0.0556 -0.0556 5.4978 -0.0667 -0.0667 5.4978 -0.0778 -0.0778 5.4978 -0.0889 -0.0889 5.4978 -0.1000 -0.1000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0011 0.0011 5.4978 --0.0022 0.0022 5.4978 --0.0033 0.0033 5.4978 --0.0044 0.0044 5.4978 --0.0056 0.0056 5.4978 --0.0067 0.0067 5.4978 --0.0078 0.0078 5.4978 --0.0089 0.0089 5.4978 --0.0100 0.0100 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 35 -25 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 25 -35 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0022 -0.0011 5.8905 -0.0044 -0.0022 5.8905 -0.0067 -0.0033 5.8905 -0.0089 -0.0044 5.8905 -0.0111 -0.0056 5.8905 -0.0133 -0.0067 5.8905 -0.0156 -0.0078 5.8905 -0.0178 -0.0089 5.8905 -0.0200 -0.0100 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 20 -10 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0222 -0.0111 5.8905 -0.0444 -0.0222 5.8905 -0.0667 -0.0333 5.8905 -0.0889 -0.0444 5.8905 -0.1111 -0.0556 5.8905 -0.1333 -0.0667 5.8905 -0.1556 -0.0778 5.8905 -0.1778 -0.0889 5.8905 -0.2000 -0.1000 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0022 0.0011 5.8905 --0.0044 0.0022 5.8905 --0.0067 0.0033 5.8905 --0.0089 0.0044 5.8905 --0.0111 0.0056 5.8905 --0.0133 0.0067 5.8905 --0.0156 0.0078 5.8905 --0.0178 0.0089 5.8905 --0.0200 0.0100 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 25 -20 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 35 -10 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim deleted file mode 100755 index 6bf962e..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.025000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0028 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0083 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0139 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0194 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0250 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 12 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0667 0.0000 0.0000 -0.1000 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1667 0.0000 0.0000 -0.2000 0.0000 0.0000 -0.2333 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0028 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0083 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0139 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0194 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0250 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 16 2 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 16 -2 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0056 0.0028 0.3927 -0.0111 0.0056 0.3927 -0.0167 0.0083 0.3927 -0.0222 0.0111 0.3927 -0.0278 0.0139 0.3927 -0.0333 0.0167 0.3927 -0.0389 0.0194 0.3927 -0.0444 0.0222 0.3927 -0.0500 0.0250 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 12 6 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0056 -0.0028 0.3927 --0.0111 -0.0056 0.3927 --0.0167 -0.0083 0.3927 --0.0222 -0.0111 0.3927 --0.0278 -0.0139 0.3927 --0.0333 -0.0167 0.3927 --0.0389 -0.0194 0.3927 --0.0444 -0.0222 0.3927 --0.0500 -0.0250 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 10 8 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 14 4 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0028 0.0028 0.7854 -0.0056 0.0056 0.7854 -0.0083 0.0083 0.7854 -0.0111 0.0111 0.7854 -0.0139 0.0139 0.7854 -0.0167 0.0167 0.7854 -0.0194 0.0194 0.7854 -0.0222 0.0222 0.7854 -0.0250 0.0250 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 12 12 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0028 -0.0028 0.7854 --0.0056 -0.0056 0.7854 --0.0083 -0.0083 0.7854 --0.0111 -0.0111 0.7854 --0.0139 -0.0139 0.7854 --0.0167 -0.0167 0.7854 --0.0194 -0.0194 0.7854 --0.0222 -0.0222 0.7854 --0.0250 -0.0250 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 10 14 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 14 10 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0028 0.0056 1.1781 -0.0056 0.0111 1.1781 -0.0083 0.0167 1.1781 -0.0111 0.0222 1.1781 -0.0139 0.0278 1.1781 -0.0167 0.0333 1.1781 -0.0194 0.0389 1.1781 -0.0222 0.0444 1.1781 -0.0250 0.0500 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 6 12 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0028 -0.0056 1.1781 --0.0056 -0.0111 1.1781 --0.0083 -0.0167 1.1781 --0.0111 -0.0222 1.1781 --0.0139 -0.0278 1.1781 --0.0167 -0.0333 1.1781 --0.0194 -0.0389 1.1781 --0.0222 -0.0444 1.1781 --0.0250 -0.0500 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 8 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 4 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0028 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0083 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0139 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0194 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0250 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 12 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0667 1.5708 -0.0000 0.1000 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1667 1.5708 -0.0000 0.2000 1.5708 -0.0000 0.2333 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0028 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0083 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0139 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0194 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0250 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -2 16 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 2 16 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0028 0.0056 1.9635 --0.0056 0.0111 1.9635 --0.0083 0.0167 1.9635 --0.0111 0.0222 1.9635 --0.0139 0.0278 1.9635 --0.0167 0.0333 1.9635 --0.0194 0.0389 1.9635 --0.0222 0.0444 1.9635 --0.0250 0.0500 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -6 12 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0028 -0.0056 1.9635 -0.0056 -0.0111 1.9635 -0.0083 -0.0167 1.9635 -0.0111 -0.0222 1.9635 -0.0139 -0.0278 1.9635 -0.0167 -0.0333 1.9635 -0.0194 -0.0389 1.9635 -0.0222 -0.0444 1.9635 -0.0250 -0.0500 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -8 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -4 14 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0028 0.0028 2.3562 --0.0056 0.0056 2.3562 --0.0083 0.0083 2.3562 --0.0111 0.0111 2.3562 --0.0139 0.0139 2.3562 --0.0167 0.0167 2.3562 --0.0194 0.0194 2.3562 --0.0222 0.0222 2.3562 --0.0250 0.0250 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -12 12 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0028 -0.0028 2.3562 -0.0056 -0.0056 2.3562 -0.0083 -0.0083 2.3562 -0.0111 -0.0111 2.3562 -0.0139 -0.0139 2.3562 -0.0167 -0.0167 2.3562 -0.0194 -0.0194 2.3562 -0.0222 -0.0222 2.3562 -0.0250 -0.0250 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -14 10 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -10 14 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0056 0.0028 2.7489 --0.0111 0.0056 2.7489 --0.0167 0.0083 2.7489 --0.0222 0.0111 2.7489 --0.0278 0.0139 2.7489 --0.0333 0.0167 2.7489 --0.0389 0.0194 2.7489 --0.0444 0.0222 2.7489 --0.0500 0.0250 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -12 6 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0056 -0.0028 2.7489 -0.0111 -0.0056 2.7489 -0.0167 -0.0083 2.7489 -0.0222 -0.0111 2.7489 -0.0278 -0.0139 2.7489 -0.0333 -0.0167 2.7489 -0.0389 -0.0194 2.7489 -0.0444 -0.0222 2.7489 -0.0500 -0.0250 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -10 8 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -14 4 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0028 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0083 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0139 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0194 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0250 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -12 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0667 0.0000 3.1416 --0.1000 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1667 0.0000 3.1416 --0.2000 0.0000 3.1416 --0.2333 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0028 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0083 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0139 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0194 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0250 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -16 -2 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -16 2 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0056 -0.0028 3.5343 --0.0111 -0.0056 3.5343 --0.0167 -0.0083 3.5343 --0.0222 -0.0111 3.5343 --0.0278 -0.0139 3.5343 --0.0333 -0.0167 3.5343 --0.0389 -0.0194 3.5343 --0.0444 -0.0222 3.5343 --0.0500 -0.0250 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -12 -6 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0056 0.0028 3.5343 -0.0111 0.0056 3.5343 -0.0167 0.0083 3.5343 -0.0222 0.0111 3.5343 -0.0278 0.0139 3.5343 -0.0333 0.0167 3.5343 -0.0389 0.0194 3.5343 -0.0444 0.0222 3.5343 -0.0500 0.0250 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -10 -8 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -14 -4 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0028 -0.0028 3.9270 --0.0056 -0.0056 3.9270 --0.0083 -0.0083 3.9270 --0.0111 -0.0111 3.9270 --0.0139 -0.0139 3.9270 --0.0167 -0.0167 3.9270 --0.0194 -0.0194 3.9270 --0.0222 -0.0222 3.9270 --0.0250 -0.0250 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -12 -12 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0028 0.0028 3.9270 -0.0056 0.0056 3.9270 -0.0083 0.0083 3.9270 -0.0111 0.0111 3.9270 -0.0139 0.0139 3.9270 -0.0167 0.0167 3.9270 -0.0194 0.0194 3.9270 -0.0222 0.0222 3.9270 -0.0250 0.0250 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -10 -14 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -14 -10 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0028 -0.0056 4.3197 --0.0056 -0.0111 4.3197 --0.0083 -0.0167 4.3197 --0.0111 -0.0222 4.3197 --0.0139 -0.0278 4.3197 --0.0167 -0.0333 4.3197 --0.0194 -0.0389 4.3197 --0.0222 -0.0444 4.3197 --0.0250 -0.0500 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -6 -12 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0028 0.0056 4.3197 -0.0056 0.0111 4.3197 -0.0083 0.0167 4.3197 -0.0111 0.0222 4.3197 -0.0139 0.0278 4.3197 -0.0167 0.0333 4.3197 -0.0194 0.0389 4.3197 -0.0222 0.0444 4.3197 -0.0250 0.0500 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -8 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -4 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0028 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0083 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0139 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0194 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0250 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -12 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0667 4.7124 -0.0000 -0.1000 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1667 4.7124 -0.0000 -0.2000 4.7124 -0.0000 -0.2333 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0028 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0083 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0139 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0194 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0250 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 2 -16 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -2 -16 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0028 -0.0056 5.1051 -0.0056 -0.0111 5.1051 -0.0083 -0.0167 5.1051 -0.0111 -0.0222 5.1051 -0.0139 -0.0278 5.1051 -0.0167 -0.0333 5.1051 -0.0194 -0.0389 5.1051 -0.0222 -0.0444 5.1051 -0.0250 -0.0500 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 6 -12 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0028 0.0056 5.1051 --0.0056 0.0111 5.1051 --0.0083 0.0167 5.1051 --0.0111 0.0222 5.1051 --0.0139 0.0278 5.1051 --0.0167 0.0333 5.1051 --0.0194 0.0389 5.1051 --0.0222 0.0444 5.1051 --0.0250 0.0500 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 8 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 4 -14 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0028 -0.0028 5.4978 -0.0056 -0.0056 5.4978 -0.0083 -0.0083 5.4978 -0.0111 -0.0111 5.4978 -0.0139 -0.0139 5.4978 -0.0167 -0.0167 5.4978 -0.0194 -0.0194 5.4978 -0.0222 -0.0222 5.4978 -0.0250 -0.0250 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 12 -12 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0028 0.0028 5.4978 --0.0056 0.0056 5.4978 --0.0083 0.0083 5.4978 --0.0111 0.0111 5.4978 --0.0139 0.0139 5.4978 --0.0167 0.0167 5.4978 --0.0194 0.0194 5.4978 --0.0222 0.0222 5.4978 --0.0250 0.0250 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 14 -10 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 10 -14 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0056 -0.0028 5.8905 -0.0111 -0.0056 5.8905 -0.0167 -0.0083 5.8905 -0.0222 -0.0111 5.8905 -0.0278 -0.0139 5.8905 -0.0333 -0.0167 5.8905 -0.0389 -0.0194 5.8905 -0.0444 -0.0222 5.8905 -0.0500 -0.0250 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 12 -6 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0056 0.0028 5.8905 --0.0111 0.0056 5.8905 --0.0167 0.0083 5.8905 --0.0222 0.0111 5.8905 --0.0278 0.0139 5.8905 --0.0333 0.0167 5.8905 --0.0389 0.0194 5.8905 --0.0444 0.0222 5.8905 --0.0500 0.0250 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 10 -8 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 14 -4 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim deleted file mode 100755 index b87c310..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.020000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0022 0.0000 0.0000 -0.0044 0.0000 0.0000 -0.0067 0.0000 0.0000 -0.0089 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0133 0.0000 0.0000 -0.0156 0.0000 0.0000 -0.0178 0.0000 0.0000 -0.0200 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 20 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0022 0.0000 0.0000 --0.0044 0.0000 0.0000 --0.0067 0.0000 0.0000 --0.0089 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0133 0.0000 0.0000 --0.0156 0.0000 0.0000 --0.0178 0.0000 0.0000 --0.0200 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 15 3 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0342 0.0008 0.0434 -0.0683 0.0031 0.0868 -0.1023 0.0069 0.1302 -0.1361 0.0121 0.1736 -0.1696 0.0188 0.2170 -0.2029 0.0270 0.2604 -0.2357 0.0366 0.3038 -0.2681 0.0476 0.3472 -0.3000 0.0600 0.3906 -primID: 4 -startangle_c: 0 -endpose_c: 15 -3 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0342 -0.0008 -0.0434 -0.0683 -0.0031 -0.0868 -0.1023 -0.0069 -0.1302 -0.1361 -0.0121 -0.1736 -0.1696 -0.0188 -0.2170 -0.2029 -0.0270 -0.2604 -0.2357 -0.0366 -0.3038 -0.2681 -0.0476 -0.3472 -0.3000 -0.0600 -0.3906 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0044 0.0022 0.3927 -0.0089 0.0044 0.3927 -0.0133 0.0067 0.3927 -0.0178 0.0089 0.3927 -0.0222 0.0111 0.3927 -0.0267 0.0133 0.3927 -0.0311 0.0156 0.3927 -0.0356 0.0178 0.3927 -0.0400 0.0200 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 15 8 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0178 0.3927 -0.0667 0.0356 0.3927 -0.1000 0.0533 0.3927 -0.1333 0.0711 0.3927 -0.1667 0.0889 0.3927 -0.2000 0.1067 0.3927 -0.2333 0.1244 0.3927 -0.2667 0.1422 0.3927 -0.3000 0.1600 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0044 -0.0022 0.3927 --0.0089 -0.0044 0.3927 --0.0133 -0.0067 0.3927 --0.0178 -0.0089 0.3927 --0.0222 -0.0111 0.3927 --0.0267 -0.0133 0.3927 --0.0311 -0.0156 0.3927 --0.0356 -0.0178 0.3927 --0.0400 -0.0200 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 12 10 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0285 0.0188 0.4210 -0.0566 0.0385 0.4492 -0.0842 0.0590 0.4775 -0.1115 0.0804 0.5057 -0.1382 0.1027 0.5340 -0.1645 0.1258 0.5623 -0.1902 0.1497 0.5905 -0.2154 0.1745 0.6188 -0.2400 0.2000 0.6470 -primID: 4 -startangle_c: 1 -endpose_c: 18 5 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0387 0.0160 0.3927 -0.0774 0.0320 0.3927 -0.1161 0.0481 0.3927 -0.1549 0.0636 0.3512 -0.1947 0.0766 0.2809 -0.2353 0.0868 0.2107 -0.2765 0.0941 0.1405 -0.3182 0.0985 0.0702 -0.3600 0.1000 0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0022 0.0022 0.7854 -0.0044 0.0044 0.7854 -0.0067 0.0067 0.7854 -0.0089 0.0089 0.7854 -0.0111 0.0111 0.7854 -0.0133 0.0133 0.7854 -0.0156 0.0156 0.7854 -0.0178 0.0178 0.7854 -0.0200 0.0200 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 15 15 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0022 -0.0022 0.7854 --0.0044 -0.0044 0.7854 --0.0067 -0.0067 0.7854 --0.0089 -0.0089 0.7854 --0.0111 -0.0111 0.7854 --0.0133 -0.0133 0.7854 --0.0156 -0.0156 0.7854 --0.0178 -0.0178 0.7854 --0.0200 -0.0200 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 12 18 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0334 0.0350 0.8288 -0.0652 0.0714 0.8722 -0.0954 0.1092 0.9156 -0.1240 0.1482 0.9590 -0.1508 0.1885 1.0024 -0.1759 0.2299 1.0458 -0.1991 0.2723 1.0892 -0.2205 0.3157 1.1326 -0.2400 0.3600 1.1760 -primID: 4 -startangle_c: 2 -endpose_c: 18 12 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0350 0.0334 0.7420 -0.0714 0.0652 0.6986 -0.1092 0.0954 0.6552 -0.1482 0.1240 0.6118 -0.1885 0.1508 0.5684 -0.2299 0.1759 0.5250 -0.2723 0.1991 0.4816 -0.3157 0.2205 0.4382 -0.3600 0.2400 0.3948 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0022 0.0044 1.1781 -0.0044 0.0089 1.1781 -0.0067 0.0133 1.1781 -0.0089 0.0178 1.1781 -0.0111 0.0222 1.1781 -0.0133 0.0267 1.1781 -0.0156 0.0311 1.1781 -0.0178 0.0356 1.1781 -0.0200 0.0400 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 8 15 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0178 0.0333 1.1781 -0.0356 0.0667 1.1781 -0.0533 0.1000 1.1781 -0.0711 0.1333 1.1781 -0.0889 0.1667 1.1781 -0.1067 0.2000 1.1781 -0.1244 0.2333 1.1781 -0.1422 0.2667 1.1781 -0.1600 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0022 -0.0044 1.1781 --0.0044 -0.0089 1.1781 --0.0067 -0.0133 1.1781 --0.0089 -0.0178 1.1781 --0.0111 -0.0222 1.1781 --0.0133 -0.0267 1.1781 --0.0156 -0.0311 1.1781 --0.0178 -0.0356 1.1781 --0.0200 -0.0400 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 10 12 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0188 0.0285 1.1498 -0.0385 0.0566 1.1216 -0.0590 0.0842 1.0933 -0.0804 0.1115 1.0651 -0.1027 0.1382 1.0368 -0.1258 0.1645 1.0085 -0.1497 0.1902 0.9803 -0.1745 0.2154 0.9520 -0.2000 0.2400 0.9238 -primID: 4 -startangle_c: 3 -endpose_c: 5 18 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0160 0.0387 1.1781 -0.0320 0.0774 1.1781 -0.0481 0.1161 1.1781 -0.0636 0.1549 1.2196 -0.0766 0.1947 1.2898 -0.0868 0.2353 1.3601 -0.0941 0.2765 1.4303 -0.0985 0.3182 1.5006 -0.1000 0.3600 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0022 1.5708 -0.0000 0.0044 1.5708 -0.0000 0.0067 1.5708 -0.0000 0.0089 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0133 1.5708 -0.0000 0.0156 1.5708 -0.0000 0.0178 1.5708 -0.0000 0.0200 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 20 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0022 1.5708 -0.0000 -0.0044 1.5708 -0.0000 -0.0067 1.5708 -0.0000 -0.0089 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0133 1.5708 -0.0000 -0.0156 1.5708 -0.0000 -0.0178 1.5708 -0.0000 -0.0200 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -3 15 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 --0.0008 0.0342 1.6142 --0.0031 0.0683 1.6576 --0.0069 0.1023 1.7010 --0.0121 0.1361 1.7444 --0.0188 0.1696 1.7878 --0.0270 0.2029 1.8312 --0.0366 0.2357 1.8746 --0.0476 0.2681 1.9180 --0.0600 0.3000 1.9614 -primID: 4 -startangle_c: 4 -endpose_c: 3 15 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0008 0.0342 1.5274 -0.0031 0.0683 1.4840 -0.0069 0.1023 1.4406 -0.0121 0.1361 1.3972 -0.0188 0.1696 1.3538 -0.0270 0.2029 1.3104 -0.0366 0.2357 1.2670 -0.0476 0.2681 1.2236 -0.0600 0.3000 1.1802 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0022 0.0044 1.9635 --0.0044 0.0089 1.9635 --0.0067 0.0133 1.9635 --0.0089 0.0178 1.9635 --0.0111 0.0222 1.9635 --0.0133 0.0267 1.9635 --0.0156 0.0311 1.9635 --0.0178 0.0356 1.9635 --0.0200 0.0400 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -8 15 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0178 0.0333 1.9635 --0.0356 0.0667 1.9635 --0.0533 0.1000 1.9635 --0.0711 0.1333 1.9635 --0.0889 0.1667 1.9635 --0.1067 0.2000 1.9635 --0.1244 0.2333 1.9635 --0.1422 0.2667 1.9635 --0.1600 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0022 -0.0044 1.9635 -0.0044 -0.0089 1.9635 -0.0067 -0.0133 1.9635 -0.0089 -0.0178 1.9635 -0.0111 -0.0222 1.9635 -0.0133 -0.0267 1.9635 -0.0156 -0.0311 1.9635 -0.0178 -0.0356 1.9635 -0.0200 -0.0400 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -10 12 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0188 0.0285 1.9918 --0.0385 0.0566 2.0200 --0.0590 0.0842 2.0483 --0.0804 0.1115 2.0765 --0.1027 0.1382 2.1048 --0.1258 0.1645 2.1330 --0.1497 0.1902 2.1613 --0.1745 0.2154 2.1896 --0.2000 0.2400 2.2178 -primID: 4 -startangle_c: 5 -endpose_c: -5 18 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0160 0.0387 1.9635 --0.0320 0.0774 1.9635 --0.0481 0.1161 1.9635 --0.0636 0.1549 1.9220 --0.0766 0.1947 1.8517 --0.0868 0.2353 1.7815 --0.0941 0.2765 1.7113 --0.0985 0.3182 1.6410 --0.1000 0.3600 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0022 0.0022 2.3562 --0.0044 0.0044 2.3562 --0.0067 0.0067 2.3562 --0.0089 0.0089 2.3562 --0.0111 0.0111 2.3562 --0.0133 0.0133 2.3562 --0.0156 0.0156 2.3562 --0.0178 0.0178 2.3562 --0.0200 0.0200 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -15 15 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0022 -0.0022 2.3562 -0.0044 -0.0044 2.3562 -0.0067 -0.0067 2.3562 -0.0089 -0.0089 2.3562 -0.0111 -0.0111 2.3562 -0.0133 -0.0133 2.3562 -0.0156 -0.0156 2.3562 -0.0178 -0.0178 2.3562 -0.0200 -0.0200 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -18 12 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0350 0.0334 2.3996 --0.0714 0.0652 2.4430 --0.1092 0.0954 2.4864 --0.1482 0.1240 2.5298 --0.1885 0.1508 2.5732 --0.2299 0.1759 2.6166 --0.2723 0.1991 2.6600 --0.3157 0.2205 2.7034 --0.3600 0.2400 2.7468 -primID: 4 -startangle_c: 6 -endpose_c: -12 18 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0334 0.0350 2.3128 --0.0652 0.0714 2.2694 --0.0954 0.1092 2.2260 --0.1240 0.1482 2.1826 --0.1508 0.1885 2.1392 --0.1759 0.2299 2.0958 --0.1991 0.2723 2.0524 --0.2205 0.3157 2.0090 --0.2400 0.3600 1.9656 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0044 0.0022 2.7489 --0.0089 0.0044 2.7489 --0.0133 0.0067 2.7489 --0.0178 0.0089 2.7489 --0.0222 0.0111 2.7489 --0.0267 0.0133 2.7489 --0.0311 0.0156 2.7489 --0.0356 0.0178 2.7489 --0.0400 0.0200 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -15 8 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0178 2.7489 --0.0667 0.0356 2.7489 --0.1000 0.0533 2.7489 --0.1333 0.0711 2.7489 --0.1667 0.0889 2.7489 --0.2000 0.1067 2.7489 --0.2333 0.1244 2.7489 --0.2667 0.1422 2.7489 --0.3000 0.1600 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0044 -0.0022 2.7489 -0.0089 -0.0044 2.7489 -0.0133 -0.0067 2.7489 -0.0178 -0.0089 2.7489 -0.0222 -0.0111 2.7489 -0.0267 -0.0133 2.7489 -0.0311 -0.0156 2.7489 -0.0356 -0.0178 2.7489 -0.0400 -0.0200 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -12 10 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0285 0.0188 2.7206 --0.0566 0.0385 2.6924 --0.0842 0.0590 2.6641 --0.1115 0.0804 2.6359 --0.1382 0.1027 2.6076 --0.1645 0.1258 2.5793 --0.1902 0.1497 2.5511 --0.2154 0.1745 2.5228 --0.2400 0.2000 2.4946 -primID: 4 -startangle_c: 7 -endpose_c: -18 5 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0387 0.0160 2.7489 --0.0774 0.0320 2.7489 --0.1161 0.0481 2.7489 --0.1549 0.0636 2.7904 --0.1947 0.0766 2.8606 --0.2353 0.0868 2.9309 --0.2765 0.0941 3.0011 --0.3182 0.0985 3.0714 --0.3600 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0022 0.0000 3.1416 --0.0044 0.0000 3.1416 --0.0067 0.0000 3.1416 --0.0089 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0133 0.0000 3.1416 --0.0156 0.0000 3.1416 --0.0178 0.0000 3.1416 --0.0200 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -20 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0022 0.0000 3.1416 -0.0044 0.0000 3.1416 -0.0067 0.0000 3.1416 -0.0089 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0133 0.0000 3.1416 -0.0156 0.0000 3.1416 -0.0178 0.0000 3.1416 -0.0200 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -15 -3 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0342 -0.0008 3.1850 --0.0683 -0.0031 3.2284 --0.1023 -0.0069 3.2718 --0.1361 -0.0121 3.3152 --0.1696 -0.0188 3.3586 --0.2029 -0.0270 3.4020 --0.2357 -0.0366 3.4454 --0.2681 -0.0476 3.4888 --0.3000 -0.0600 3.5322 -primID: 4 -startangle_c: 8 -endpose_c: -15 3 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0342 0.0008 3.0982 --0.0683 0.0031 3.0548 --0.1023 0.0069 3.0114 --0.1361 0.0121 2.9680 --0.1696 0.0188 2.9246 --0.2029 0.0270 2.8812 --0.2357 0.0366 2.8378 --0.2681 0.0476 2.7944 --0.3000 0.0600 2.7510 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0044 -0.0022 3.5343 --0.0089 -0.0044 3.5343 --0.0133 -0.0067 3.5343 --0.0178 -0.0089 3.5343 --0.0222 -0.0111 3.5343 --0.0267 -0.0133 3.5343 --0.0311 -0.0156 3.5343 --0.0356 -0.0178 3.5343 --0.0400 -0.0200 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -15 -8 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0178 3.5343 --0.0667 -0.0356 3.5343 --0.1000 -0.0533 3.5343 --0.1333 -0.0711 3.5343 --0.1667 -0.0889 3.5343 --0.2000 -0.1067 3.5343 --0.2333 -0.1244 3.5343 --0.2667 -0.1422 3.5343 --0.3000 -0.1600 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0044 0.0022 3.5343 -0.0089 0.0044 3.5343 -0.0133 0.0067 3.5343 -0.0178 0.0089 3.5343 -0.0222 0.0111 3.5343 -0.0267 0.0133 3.5343 -0.0311 0.0156 3.5343 -0.0356 0.0178 3.5343 -0.0400 0.0200 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -12 -10 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0285 -0.0188 3.5626 --0.0566 -0.0385 3.5908 --0.0842 -0.0590 3.6191 --0.1115 -0.0804 3.6473 --0.1382 -0.1027 3.6756 --0.1645 -0.1258 3.7038 --0.1902 -0.1497 3.7321 --0.2154 -0.1745 3.7604 --0.2400 -0.2000 3.7886 -primID: 4 -startangle_c: 9 -endpose_c: -18 -5 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0387 -0.0160 3.5343 --0.0774 -0.0320 3.5343 --0.1161 -0.0481 3.5343 --0.1549 -0.0636 3.4928 --0.1947 -0.0766 3.4225 --0.2353 -0.0868 3.3523 --0.2765 -0.0941 3.2821 --0.3182 -0.0985 3.2118 --0.3600 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0022 -0.0022 3.9270 --0.0044 -0.0044 3.9270 --0.0067 -0.0067 3.9270 --0.0089 -0.0089 3.9270 --0.0111 -0.0111 3.9270 --0.0133 -0.0133 3.9270 --0.0156 -0.0156 3.9270 --0.0178 -0.0178 3.9270 --0.0200 -0.0200 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -15 -15 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0022 0.0022 3.9270 -0.0044 0.0044 3.9270 -0.0067 0.0067 3.9270 -0.0089 0.0089 3.9270 -0.0111 0.0111 3.9270 -0.0133 0.0133 3.9270 -0.0156 0.0156 3.9270 -0.0178 0.0178 3.9270 -0.0200 0.0200 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -12 -18 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0334 -0.0350 3.9704 --0.0652 -0.0714 4.0138 --0.0954 -0.1092 4.0572 --0.1240 -0.1482 4.1006 --0.1508 -0.1885 4.1440 --0.1759 -0.2299 4.1874 --0.1991 -0.2723 4.2308 --0.2205 -0.3157 4.2742 --0.2400 -0.3600 4.3176 -primID: 4 -startangle_c: 10 -endpose_c: -18 -12 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0350 -0.0334 3.8836 --0.0714 -0.0652 3.8402 --0.1092 -0.0954 3.7968 --0.1482 -0.1240 3.7534 --0.1885 -0.1508 3.7100 --0.2299 -0.1759 3.6666 --0.2723 -0.1991 3.6232 --0.3157 -0.2205 3.5798 --0.3600 -0.2400 3.5364 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0022 -0.0044 4.3197 --0.0044 -0.0089 4.3197 --0.0067 -0.0133 4.3197 --0.0089 -0.0178 4.3197 --0.0111 -0.0222 4.3197 --0.0133 -0.0267 4.3197 --0.0156 -0.0311 4.3197 --0.0178 -0.0356 4.3197 --0.0200 -0.0400 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -8 -15 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0178 -0.0333 4.3197 --0.0356 -0.0667 4.3197 --0.0533 -0.1000 4.3197 --0.0711 -0.1333 4.3197 --0.0889 -0.1667 4.3197 --0.1067 -0.2000 4.3197 --0.1244 -0.2333 4.3197 --0.1422 -0.2667 4.3197 --0.1600 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0022 0.0044 4.3197 -0.0044 0.0089 4.3197 -0.0067 0.0133 4.3197 -0.0089 0.0178 4.3197 -0.0111 0.0222 4.3197 -0.0133 0.0267 4.3197 -0.0156 0.0311 4.3197 -0.0178 0.0356 4.3197 -0.0200 0.0400 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -10 -12 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0188 -0.0285 4.2914 --0.0385 -0.0566 4.2632 --0.0590 -0.0842 4.2349 --0.0804 -0.1115 4.2067 --0.1027 -0.1382 4.1784 --0.1258 -0.1645 4.1501 --0.1497 -0.1902 4.1219 --0.1745 -0.2154 4.0936 --0.2000 -0.2400 4.0654 -primID: 4 -startangle_c: 11 -endpose_c: -5 -18 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0160 -0.0387 4.3197 --0.0320 -0.0774 4.3197 --0.0481 -0.1161 4.3197 --0.0636 -0.1549 4.3612 --0.0766 -0.1947 4.4314 --0.0868 -0.2353 4.5017 --0.0941 -0.2765 4.5719 --0.0985 -0.3182 4.6422 --0.1000 -0.3600 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0022 4.7124 -0.0000 -0.0044 4.7124 -0.0000 -0.0067 4.7124 -0.0000 -0.0089 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0133 4.7124 -0.0000 -0.0156 4.7124 -0.0000 -0.0178 4.7124 -0.0000 -0.0200 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -20 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0022 4.7124 -0.0000 0.0044 4.7124 -0.0000 0.0067 4.7124 -0.0000 0.0089 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0133 4.7124 -0.0000 0.0156 4.7124 -0.0000 0.0178 4.7124 -0.0000 0.0200 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 3 -15 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0008 -0.0342 4.7558 -0.0031 -0.0683 4.7992 -0.0069 -0.1023 4.8426 -0.0121 -0.1361 4.8860 -0.0188 -0.1696 4.9294 -0.0270 -0.2029 4.9728 -0.0366 -0.2357 5.0162 -0.0476 -0.2681 5.0596 -0.0600 -0.3000 5.1030 -primID: 4 -startangle_c: 12 -endpose_c: -3 -15 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0008 -0.0342 4.6690 --0.0031 -0.0683 4.6256 --0.0069 -0.1023 4.5822 --0.0121 -0.1361 4.5388 --0.0188 -0.1696 4.4954 --0.0270 -0.2029 4.4520 --0.0366 -0.2357 4.4086 --0.0476 -0.2681 4.3652 --0.0600 -0.3000 4.3218 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0022 -0.0044 5.1051 -0.0044 -0.0089 5.1051 -0.0067 -0.0133 5.1051 -0.0089 -0.0178 5.1051 -0.0111 -0.0222 5.1051 -0.0133 -0.0267 5.1051 -0.0156 -0.0311 5.1051 -0.0178 -0.0356 5.1051 -0.0200 -0.0400 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 8 -15 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0178 -0.0333 5.1051 -0.0356 -0.0667 5.1051 -0.0533 -0.1000 5.1051 -0.0711 -0.1333 5.1051 -0.0889 -0.1667 5.1051 -0.1067 -0.2000 5.1051 -0.1244 -0.2333 5.1051 -0.1422 -0.2667 5.1051 -0.1600 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0022 0.0044 5.1051 --0.0044 0.0089 5.1051 --0.0067 0.0133 5.1051 --0.0089 0.0178 5.1051 --0.0111 0.0222 5.1051 --0.0133 0.0267 5.1051 --0.0156 0.0311 5.1051 --0.0178 0.0356 5.1051 --0.0200 0.0400 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 10 -12 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0188 -0.0285 5.1333 -0.0385 -0.0566 5.1616 -0.0590 -0.0842 5.1899 -0.0804 -0.1115 5.2181 -0.1027 -0.1382 5.2464 -0.1258 -0.1645 5.2746 -0.1497 -0.1902 5.3029 -0.1745 -0.2154 5.3312 -0.2000 -0.2400 5.3594 -primID: 4 -startangle_c: 13 -endpose_c: 5 -18 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0160 -0.0387 5.1051 -0.0320 -0.0774 5.1051 -0.0481 -0.1161 5.1051 -0.0636 -0.1549 5.0636 -0.0766 -0.1947 4.9933 -0.0868 -0.2353 4.9231 -0.0941 -0.2765 4.8529 -0.0985 -0.3182 4.7826 -0.1000 -0.3600 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0022 -0.0022 5.4978 -0.0044 -0.0044 5.4978 -0.0067 -0.0067 5.4978 -0.0089 -0.0089 5.4978 -0.0111 -0.0111 5.4978 -0.0133 -0.0133 5.4978 -0.0156 -0.0156 5.4978 -0.0178 -0.0178 5.4978 -0.0200 -0.0200 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 15 -15 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0022 0.0022 5.4978 --0.0044 0.0044 5.4978 --0.0067 0.0067 5.4978 --0.0089 0.0089 5.4978 --0.0111 0.0111 5.4978 --0.0133 0.0133 5.4978 --0.0156 0.0156 5.4978 --0.0178 0.0178 5.4978 --0.0200 0.0200 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 18 -12 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0350 -0.0334 5.5412 -0.0714 -0.0652 5.5846 -0.1092 -0.0954 5.6280 -0.1482 -0.1240 5.6714 -0.1885 -0.1508 5.7148 -0.2299 -0.1759 5.7582 -0.2723 -0.1991 5.8016 -0.3157 -0.2205 5.8450 -0.3600 -0.2400 5.8884 -primID: 4 -startangle_c: 14 -endpose_c: 12 -18 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0334 -0.0350 5.4544 -0.0652 -0.0714 5.4110 -0.0954 -0.1092 5.3676 -0.1240 -0.1482 5.3242 -0.1508 -0.1885 5.2808 -0.1759 -0.2299 5.2374 -0.1991 -0.2723 5.1940 -0.2205 -0.3157 5.1506 -0.2400 -0.3600 5.1072 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 10 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0044 -0.0022 5.8905 -0.0089 -0.0044 5.8905 -0.0133 -0.0067 5.8905 -0.0178 -0.0089 5.8905 -0.0222 -0.0111 5.8905 -0.0267 -0.0133 5.8905 -0.0311 -0.0156 5.8905 -0.0356 -0.0178 5.8905 -0.0400 -0.0200 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 15 -8 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0178 5.8905 -0.0667 -0.0356 5.8905 -0.1000 -0.0533 5.8905 -0.1333 -0.0711 5.8905 -0.1667 -0.0889 5.8905 -0.2000 -0.1067 5.8905 -0.2333 -0.1244 5.8905 -0.2667 -0.1422 5.8905 -0.3000 -0.1600 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 50 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0044 0.0022 5.8905 --0.0089 0.0044 5.8905 --0.0133 0.0067 5.8905 --0.0178 0.0089 5.8905 --0.0222 0.0111 5.8905 --0.0267 0.0133 5.8905 --0.0311 0.0156 5.8905 --0.0356 0.0178 5.8905 --0.0400 0.0200 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 12 -10 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0285 -0.0188 5.8622 -0.0566 -0.0385 5.8340 -0.0842 -0.0590 5.8057 -0.1115 -0.0804 5.7775 -0.1382 -0.1027 5.7492 -0.1645 -0.1258 5.7209 -0.1902 -0.1497 5.6927 -0.2154 -0.1745 5.6644 -0.2400 -0.2000 5.6362 -primID: 4 -startangle_c: 15 -endpose_c: 18 -5 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0387 -0.0160 5.8905 -0.0774 -0.0320 5.8905 -0.1161 -0.0481 5.8905 -0.1549 -0.0636 5.9320 -0.1947 -0.0766 6.0022 -0.2353 -0.0868 6.0725 -0.2765 -0.0941 6.1427 -0.3182 -0.0985 6.2129 -0.3600 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 100 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.2360 -0.0000 0.0000 4.5815 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.2725 -0.0000 0.0000 2.6180 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.3090 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim deleted file mode 100755 index bff2688..0000000 --- a/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim +++ /dev/null @@ -1,1683 +0,0 @@ -resolution_m: 0.050000 -numberofangles: 16 -totalnumberofprimitives: 112 -primID: 0 -startangle_c: 0 -endpose_c: 1 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0056 0.0000 0.0000 -0.0111 0.0000 0.0000 -0.0167 0.0000 0.0000 -0.0222 0.0000 0.0000 -0.0278 0.0000 0.0000 -0.0333 0.0000 0.0000 -0.0389 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0500 0.0000 0.0000 -primID: 1 -startangle_c: 0 -endpose_c: 8 0 0 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0444 0.0000 0.0000 -0.0889 0.0000 0.0000 -0.1333 0.0000 0.0000 -0.1778 0.0000 0.0000 -0.2222 0.0000 0.0000 -0.2667 0.0000 0.0000 -0.3111 0.0000 0.0000 -0.3556 0.0000 0.0000 -0.4000 0.0000 0.0000 -primID: 2 -startangle_c: 0 -endpose_c: -1 0 0 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.0000 --0.0056 0.0000 0.0000 --0.0111 0.0000 0.0000 --0.0167 0.0000 0.0000 --0.0222 0.0000 0.0000 --0.0278 0.0000 0.0000 --0.0333 0.0000 0.0000 --0.0389 0.0000 0.0000 --0.0444 0.0000 0.0000 --0.0500 0.0000 0.0000 -primID: 3 -startangle_c: 0 -endpose_c: 8 1 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 -0.0000 0.0000 -0.0904 -0.0000 0.0000 -0.1355 -0.0000 0.0000 -0.1807 0.0008 0.0488 -0.2257 0.0045 0.1176 -0.2703 0.0114 0.1864 -0.3144 0.0213 0.2551 -0.3577 0.0342 0.3239 -0.4000 0.0500 0.3927 -primID: 4 -startangle_c: 0 -endpose_c: 8 -1 -1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0452 0.0000 0.0000 -0.0904 0.0000 0.0000 -0.1355 0.0000 0.0000 -0.1807 -0.0008 -0.0488 -0.2257 -0.0045 -0.1176 -0.2703 -0.0114 -0.1864 -0.3144 -0.0213 -0.2551 -0.3577 -0.0342 -0.3239 -0.4000 -0.0500 -0.3927 -primID: 5 -startangle_c: 0 -endpose_c: 0 0 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3927 -primID: 6 -startangle_c: 0 -endpose_c: 0 0 -1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.0000 -0.0000 0.0000 -0.0436 -0.0000 0.0000 -0.0873 -0.0000 0.0000 -0.1309 -0.0000 0.0000 -0.1745 -0.0000 0.0000 -0.2182 -0.0000 0.0000 -0.2618 -0.0000 0.0000 -0.3054 -0.0000 0.0000 -0.3491 -0.0000 0.0000 -0.3927 -primID: 0 -startangle_c: 1 -endpose_c: 2 1 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0111 0.0056 0.3927 -0.0222 0.0111 0.3927 -0.0333 0.0167 0.3927 -0.0444 0.0222 0.3927 -0.0556 0.0278 0.3927 -0.0667 0.0333 0.3927 -0.0778 0.0389 0.3927 -0.0889 0.0444 0.3927 -0.1000 0.0500 0.3927 -primID: 1 -startangle_c: 1 -endpose_c: 6 3 1 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0333 0.0167 0.3927 -0.0667 0.0333 0.3927 -0.1000 0.0500 0.3927 -0.1333 0.0667 0.3927 -0.1667 0.0833 0.3927 -0.2000 0.1000 0.3927 -0.2333 0.1167 0.3927 -0.2667 0.1333 0.3927 -0.3000 0.1500 0.3927 -primID: 2 -startangle_c: 1 -endpose_c: -2 -1 1 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.3927 --0.0111 -0.0056 0.3927 --0.0222 -0.0111 0.3927 --0.0333 -0.0167 0.3927 --0.0444 -0.0222 0.3927 --0.0556 -0.0278 0.3927 --0.0667 -0.0333 0.3927 --0.0778 -0.0389 0.3927 --0.0889 -0.0444 0.3927 --0.1000 -0.0500 0.3927 -primID: 3 -startangle_c: 1 -endpose_c: 5 4 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0298 0.0184 0.4230 -0.0592 0.0379 0.4533 -0.0881 0.0583 0.4836 -0.1165 0.0796 0.5139 -0.1444 0.1019 0.5442 -0.1717 0.1251 0.5745 -0.1984 0.1492 0.6048 -0.2245 0.1742 0.6351 -0.2500 0.2000 0.6654 -primID: 4 -startangle_c: 1 -endpose_c: 7 2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0377 0.0156 0.3927 -0.0754 0.0312 0.3927 -0.1131 0.0468 0.3927 -0.1508 0.0623 0.3736 -0.1893 0.0758 0.2989 -0.2287 0.0863 0.2242 -0.2687 0.0939 0.1494 -0.3092 0.0985 0.0747 -0.3500 0.1000 -0.0000 -primID: 5 -startangle_c: 1 -endpose_c: 0 0 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 1 -endpose_c: 0 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.3927 -0.0000 0.0000 0.3491 -0.0000 0.0000 0.3054 -0.0000 0.0000 0.2618 -0.0000 0.0000 0.2182 -0.0000 0.0000 0.1745 -0.0000 0.0000 0.1309 -0.0000 0.0000 0.0873 -0.0000 0.0000 0.0436 -0.0000 0.0000 0.0000 -primID: 0 -startangle_c: 2 -endpose_c: 1 1 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0056 0.0056 0.7854 -0.0111 0.0111 0.7854 -0.0167 0.0167 0.7854 -0.0222 0.0222 0.7854 -0.0278 0.0278 0.7854 -0.0333 0.0333 0.7854 -0.0389 0.0389 0.7854 -0.0444 0.0444 0.7854 -0.0500 0.0500 0.7854 -primID: 1 -startangle_c: 2 -endpose_c: 6 6 2 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0333 0.0333 0.7854 -0.0667 0.0667 0.7854 -0.1000 0.1000 0.7854 -0.1333 0.1333 0.7854 -0.1667 0.1667 0.7854 -0.2000 0.2000 0.7854 -0.2333 0.2333 0.7854 -0.2667 0.2667 0.7854 -0.3000 0.3000 0.7854 -primID: 2 -startangle_c: 2 -endpose_c: -1 -1 2 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 0.7854 --0.0056 -0.0056 0.7854 --0.0111 -0.0111 0.7854 --0.0167 -0.0167 0.7854 --0.0222 -0.0222 0.7854 --0.0278 -0.0278 0.7854 --0.0333 -0.0333 0.7854 --0.0389 -0.0389 0.7854 --0.0444 -0.0444 0.7854 --0.0500 -0.0500 0.7854 -primID: 3 -startangle_c: 2 -endpose_c: 5 7 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0678 0.0684 0.8151 -0.1000 0.1043 0.8669 -0.1302 0.1418 0.9188 -0.1584 0.1809 0.9707 -0.1846 0.2213 1.0225 -0.2086 0.2631 1.0744 -0.2304 0.3060 1.1262 -0.2500 0.3500 1.1781 -primID: 4 -startangle_c: 2 -endpose_c: 7 5 1 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0341 0.0341 0.7854 -0.0684 0.0678 0.7557 -0.1043 0.1000 0.7039 -0.1418 0.1302 0.6520 -0.1809 0.1584 0.6001 -0.2213 0.1846 0.5483 -0.2631 0.2086 0.4964 -0.3060 0.2304 0.4446 -0.3500 0.2500 0.3927 -primID: 5 -startangle_c: 2 -endpose_c: 0 0 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.9599 -0.0000 0.0000 1.0036 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.1781 -primID: 6 -startangle_c: 2 -endpose_c: 0 0 1 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 0.7854 -0.0000 0.0000 0.7418 -0.0000 0.0000 0.6981 -0.0000 0.0000 0.6545 -0.0000 0.0000 0.6109 -0.0000 0.0000 0.5672 -0.0000 0.0000 0.5236 -0.0000 0.0000 0.4800 -0.0000 0.0000 0.4363 -0.0000 0.0000 0.3927 -primID: 0 -startangle_c: 3 -endpose_c: 1 2 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0056 0.0111 1.1781 -0.0111 0.0222 1.1781 -0.0167 0.0333 1.1781 -0.0222 0.0444 1.1781 -0.0278 0.0556 1.1781 -0.0333 0.0667 1.1781 -0.0389 0.0778 1.1781 -0.0444 0.0889 1.1781 -0.0500 0.1000 1.1781 -primID: 1 -startangle_c: 3 -endpose_c: 3 6 3 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0167 0.0333 1.1781 -0.0333 0.0667 1.1781 -0.0500 0.1000 1.1781 -0.0667 0.1333 1.1781 -0.0833 0.1667 1.1781 -0.1000 0.2000 1.1781 -0.1167 0.2333 1.1781 -0.1333 0.2667 1.1781 -0.1500 0.3000 1.1781 -primID: 2 -startangle_c: 3 -endpose_c: -1 -2 3 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.1781 --0.0056 -0.0111 1.1781 --0.0111 -0.0222 1.1781 --0.0167 -0.0333 1.1781 --0.0222 -0.0444 1.1781 --0.0278 -0.0556 1.1781 --0.0333 -0.0667 1.1781 --0.0389 -0.0778 1.1781 --0.0444 -0.0889 1.1781 --0.0500 -0.1000 1.1781 -primID: 3 -startangle_c: 3 -endpose_c: 4 5 2 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0184 0.0298 1.1478 -0.0379 0.0592 1.1175 -0.0583 0.0881 1.0872 -0.0796 0.1165 1.0569 -0.1019 0.1444 1.0266 -0.1251 0.1717 0.9963 -0.1492 0.1984 0.9660 -0.1742 0.2245 0.9357 -0.2000 0.2500 0.9054 -primID: 4 -startangle_c: 3 -endpose_c: 2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0156 0.0377 1.1781 -0.0312 0.0754 1.1781 -0.0468 0.1131 1.1781 -0.0623 0.1508 1.1972 -0.0758 0.1893 1.2719 -0.0863 0.2287 1.3466 -0.0939 0.2687 1.4214 -0.0985 0.3092 1.4961 -0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 3 -endpose_c: 0 0 2 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.1345 -0.0000 0.0000 1.0908 -0.0000 0.0000 1.0472 -0.0000 0.0000 1.0036 -0.0000 0.0000 0.9599 -0.0000 0.0000 0.9163 -0.0000 0.0000 0.8727 -0.0000 0.0000 0.8290 -0.0000 0.0000 0.7854 -primID: 6 -startangle_c: 3 -endpose_c: 0 0 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.1781 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 4 -endpose_c: 0 1 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0056 1.5708 -0.0000 0.0111 1.5708 -0.0000 0.0167 1.5708 -0.0000 0.0222 1.5708 -0.0000 0.0278 1.5708 -0.0000 0.0333 1.5708 -0.0000 0.0389 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0500 1.5708 -primID: 1 -startangle_c: 4 -endpose_c: 0 8 4 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0444 1.5708 -0.0000 0.0889 1.5708 -0.0000 0.1333 1.5708 -0.0000 0.1778 1.5708 -0.0000 0.2222 1.5708 -0.0000 0.2667 1.5708 -0.0000 0.3111 1.5708 -0.0000 0.3556 1.5708 -0.0000 0.4000 1.5708 -primID: 2 -startangle_c: 4 -endpose_c: 0 -1 4 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 -0.0056 1.5708 -0.0000 -0.0111 1.5708 -0.0000 -0.0167 1.5708 -0.0000 -0.0222 1.5708 -0.0000 -0.0278 1.5708 -0.0000 -0.0333 1.5708 -0.0000 -0.0389 1.5708 -0.0000 -0.0444 1.5708 -0.0000 -0.0500 1.5708 -primID: 3 -startangle_c: 4 -endpose_c: -1 8 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 --0.0008 0.1807 1.6196 --0.0045 0.2257 1.6884 --0.0114 0.2703 1.7572 --0.0213 0.3144 1.8259 --0.0342 0.3577 1.8947 --0.0500 0.4000 1.9635 -primID: 4 -startangle_c: 4 -endpose_c: 1 8 3 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0452 1.5708 -0.0000 0.0904 1.5708 -0.0000 0.1355 1.5708 -0.0008 0.1807 1.5220 -0.0045 0.2257 1.4532 -0.0114 0.2703 1.3844 -0.0213 0.3144 1.3156 -0.0342 0.3577 1.2469 -0.0500 0.4000 1.1781 -primID: 5 -startangle_c: 4 -endpose_c: 0 0 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.9635 -primID: 6 -startangle_c: 4 -endpose_c: 0 0 3 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.5708 -0.0000 0.0000 1.5272 -0.0000 0.0000 1.4835 -0.0000 0.0000 1.4399 -0.0000 0.0000 1.3963 -0.0000 0.0000 1.3526 -0.0000 0.0000 1.3090 -0.0000 0.0000 1.2654 -0.0000 0.0000 1.2217 -0.0000 0.0000 1.1781 -primID: 0 -startangle_c: 5 -endpose_c: -1 2 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0056 0.0111 1.9635 --0.0111 0.0222 1.9635 --0.0167 0.0333 1.9635 --0.0222 0.0444 1.9635 --0.0278 0.0556 1.9635 --0.0333 0.0667 1.9635 --0.0389 0.0778 1.9635 --0.0444 0.0889 1.9635 --0.0500 0.1000 1.9635 -primID: 1 -startangle_c: 5 -endpose_c: -3 6 5 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0167 0.0333 1.9635 --0.0333 0.0667 1.9635 --0.0500 0.1000 1.9635 --0.0667 0.1333 1.9635 --0.0833 0.1667 1.9635 --0.1000 0.2000 1.9635 --0.1167 0.2333 1.9635 --0.1333 0.2667 1.9635 --0.1500 0.3000 1.9635 -primID: 2 -startangle_c: 5 -endpose_c: 1 -2 5 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0056 -0.0111 1.9635 -0.0111 -0.0222 1.9635 -0.0167 -0.0333 1.9635 -0.0222 -0.0444 1.9635 -0.0278 -0.0556 1.9635 -0.0333 -0.0667 1.9635 -0.0389 -0.0778 1.9635 -0.0444 -0.0889 1.9635 -0.0500 -0.1000 1.9635 -primID: 3 -startangle_c: 5 -endpose_c: -4 5 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0184 0.0298 1.9938 --0.0379 0.0592 2.0241 --0.0583 0.0881 2.0544 --0.0796 0.1165 2.0847 --0.1019 0.1444 2.1150 --0.1251 0.1717 2.1453 --0.1492 0.1984 2.1756 --0.1742 0.2245 2.2059 --0.2000 0.2500 2.2362 -primID: 4 -startangle_c: 5 -endpose_c: -2 7 4 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 1.9635 --0.0156 0.0377 1.9635 --0.0312 0.0754 1.9635 --0.0468 0.1131 1.9635 --0.0623 0.1508 1.9444 --0.0758 0.1893 1.8697 --0.0863 0.2287 1.7950 --0.0939 0.2687 1.7202 --0.0985 0.3092 1.6455 --0.1000 0.3500 1.5708 -primID: 5 -startangle_c: 5 -endpose_c: 0 0 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 2.0071 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 5 -endpose_c: 0 0 4 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 1.9635 -0.0000 0.0000 1.9199 -0.0000 0.0000 1.8762 -0.0000 0.0000 1.8326 -0.0000 0.0000 1.7890 -0.0000 0.0000 1.7453 -0.0000 0.0000 1.7017 -0.0000 0.0000 1.6581 -0.0000 0.0000 1.6144 -0.0000 0.0000 1.5708 -primID: 0 -startangle_c: 6 -endpose_c: -1 1 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0056 0.0056 2.3562 --0.0111 0.0111 2.3562 --0.0167 0.0167 2.3562 --0.0222 0.0222 2.3562 --0.0278 0.0278 2.3562 --0.0333 0.0333 2.3562 --0.0389 0.0389 2.3562 --0.0444 0.0444 2.3562 --0.0500 0.0500 2.3562 -primID: 1 -startangle_c: 6 -endpose_c: -6 6 6 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0333 0.0333 2.3562 --0.0667 0.0667 2.3562 --0.1000 0.1000 2.3562 --0.1333 0.1333 2.3562 --0.1667 0.1667 2.3562 --0.2000 0.2000 2.3562 --0.2333 0.2333 2.3562 --0.2667 0.2667 2.3562 --0.3000 0.3000 2.3562 -primID: 2 -startangle_c: 6 -endpose_c: 1 -1 6 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0056 -0.0056 2.3562 -0.0111 -0.0111 2.3562 -0.0167 -0.0167 2.3562 -0.0222 -0.0222 2.3562 -0.0278 -0.0278 2.3562 -0.0333 -0.0333 2.3562 -0.0389 -0.0389 2.3562 -0.0444 -0.0444 2.3562 -0.0500 -0.0500 2.3562 -primID: 3 -startangle_c: 6 -endpose_c: -7 5 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0684 0.0678 2.3859 --0.1043 0.1000 2.4377 --0.1418 0.1302 2.4896 --0.1809 0.1584 2.5415 --0.2213 0.1846 2.5933 --0.2631 0.2086 2.6452 --0.3060 0.2304 2.6970 --0.3500 0.2500 2.7489 -primID: 4 -startangle_c: 6 -endpose_c: -5 7 5 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.3562 --0.0341 0.0341 2.3562 --0.0678 0.0684 2.3265 --0.1000 0.1043 2.2747 --0.1302 0.1418 2.2228 --0.1584 0.1809 2.1709 --0.1846 0.2213 2.1191 --0.2086 0.2631 2.0672 --0.2304 0.3060 2.0154 --0.2500 0.3500 1.9635 -primID: 5 -startangle_c: 6 -endpose_c: 0 0 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.7489 -primID: 6 -startangle_c: 6 -endpose_c: 0 0 5 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.3562 -0.0000 0.0000 2.3126 -0.0000 0.0000 2.2689 -0.0000 0.0000 2.2253 -0.0000 0.0000 2.1817 -0.0000 0.0000 2.1380 -0.0000 0.0000 2.0944 -0.0000 0.0000 2.0508 -0.0000 0.0000 2.0071 -0.0000 0.0000 1.9635 -primID: 0 -startangle_c: 7 -endpose_c: -2 1 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0111 0.0056 2.7489 --0.0222 0.0111 2.7489 --0.0333 0.0167 2.7489 --0.0444 0.0222 2.7489 --0.0556 0.0278 2.7489 --0.0667 0.0333 2.7489 --0.0778 0.0389 2.7489 --0.0889 0.0444 2.7489 --0.1000 0.0500 2.7489 -primID: 1 -startangle_c: 7 -endpose_c: -6 3 7 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0333 0.0167 2.7489 --0.0667 0.0333 2.7489 --0.1000 0.0500 2.7489 --0.1333 0.0667 2.7489 --0.1667 0.0833 2.7489 --0.2000 0.1000 2.7489 --0.2333 0.1167 2.7489 --0.2667 0.1333 2.7489 --0.3000 0.1500 2.7489 -primID: 2 -startangle_c: 7 -endpose_c: 2 -1 7 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0111 -0.0056 2.7489 -0.0222 -0.0111 2.7489 -0.0333 -0.0167 2.7489 -0.0444 -0.0222 2.7489 -0.0556 -0.0278 2.7489 -0.0667 -0.0333 2.7489 -0.0778 -0.0389 2.7489 -0.0889 -0.0444 2.7489 -0.1000 -0.0500 2.7489 -primID: 3 -startangle_c: 7 -endpose_c: -5 4 6 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0298 0.0184 2.7186 --0.0592 0.0379 2.6883 --0.0881 0.0583 2.6580 --0.1165 0.0796 2.6277 --0.1444 0.1019 2.5974 --0.1717 0.1251 2.5671 --0.1984 0.1492 2.5368 --0.2245 0.1742 2.5065 --0.2500 0.2000 2.4762 -primID: 4 -startangle_c: 7 -endpose_c: -7 2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 2.7489 --0.0377 0.0156 2.7489 --0.0754 0.0312 2.7489 --0.1131 0.0468 2.7489 --0.1508 0.0623 2.7680 --0.1893 0.0758 2.8427 --0.2287 0.0863 2.9174 --0.2687 0.0939 2.9921 --0.3092 0.0985 3.0669 --0.3500 0.1000 3.1416 -primID: 5 -startangle_c: 7 -endpose_c: 0 0 6 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7053 -0.0000 0.0000 2.6616 -0.0000 0.0000 2.6180 -0.0000 0.0000 2.5744 -0.0000 0.0000 2.5307 -0.0000 0.0000 2.4871 -0.0000 0.0000 2.4435 -0.0000 0.0000 2.3998 -0.0000 0.0000 2.3562 -primID: 6 -startangle_c: 7 -endpose_c: 0 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 2.7489 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.9671 -0.0000 0.0000 3.0107 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 8 -endpose_c: -1 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0056 0.0000 3.1416 --0.0111 0.0000 3.1416 --0.0167 0.0000 3.1416 --0.0222 0.0000 3.1416 --0.0278 0.0000 3.1416 --0.0333 0.0000 3.1416 --0.0389 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0500 0.0000 3.1416 -primID: 1 -startangle_c: 8 -endpose_c: -8 0 8 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0444 0.0000 3.1416 --0.0889 0.0000 3.1416 --0.1333 0.0000 3.1416 --0.1778 0.0000 3.1416 --0.2222 0.0000 3.1416 --0.2667 0.0000 3.1416 --0.3111 0.0000 3.1416 --0.3556 0.0000 3.1416 --0.4000 0.0000 3.1416 -primID: 2 -startangle_c: 8 -endpose_c: 1 0 8 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0056 0.0000 3.1416 -0.0111 0.0000 3.1416 -0.0167 0.0000 3.1416 -0.0222 0.0000 3.1416 -0.0278 0.0000 3.1416 -0.0333 0.0000 3.1416 -0.0389 0.0000 3.1416 -0.0444 0.0000 3.1416 -0.0500 0.0000 3.1416 -primID: 3 -startangle_c: 8 -endpose_c: -8 -1 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 -0.0008 3.1904 --0.2257 -0.0045 3.2592 --0.2703 -0.0114 3.3280 --0.3144 -0.0213 3.3967 --0.3577 -0.0342 3.4655 --0.4000 -0.0500 3.5343 -primID: 4 -startangle_c: 8 -endpose_c: -8 1 7 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.1416 --0.0452 0.0000 3.1416 --0.0904 0.0000 3.1416 --0.1355 0.0000 3.1416 --0.1807 0.0008 3.0928 --0.2257 0.0045 3.0240 --0.2703 0.0114 2.9552 --0.3144 0.0213 2.8864 --0.3577 0.0342 2.8177 --0.4000 0.0500 2.7489 -primID: 5 -startangle_c: 8 -endpose_c: 0 0 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.5343 -primID: 6 -startangle_c: 8 -endpose_c: 0 0 7 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.1416 -0.0000 0.0000 3.0980 -0.0000 0.0000 3.0543 -0.0000 0.0000 3.0107 -0.0000 0.0000 2.9671 -0.0000 0.0000 2.9234 -0.0000 0.0000 2.8798 -0.0000 0.0000 2.8362 -0.0000 0.0000 2.7925 -0.0000 0.0000 2.7489 -primID: 0 -startangle_c: 9 -endpose_c: -2 -1 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0111 -0.0056 3.5343 --0.0222 -0.0111 3.5343 --0.0333 -0.0167 3.5343 --0.0444 -0.0222 3.5343 --0.0556 -0.0278 3.5343 --0.0667 -0.0333 3.5343 --0.0778 -0.0389 3.5343 --0.0889 -0.0444 3.5343 --0.1000 -0.0500 3.5343 -primID: 1 -startangle_c: 9 -endpose_c: -6 -3 9 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0333 -0.0167 3.5343 --0.0667 -0.0333 3.5343 --0.1000 -0.0500 3.5343 --0.1333 -0.0667 3.5343 --0.1667 -0.0833 3.5343 --0.2000 -0.1000 3.5343 --0.2333 -0.1167 3.5343 --0.2667 -0.1333 3.5343 --0.3000 -0.1500 3.5343 -primID: 2 -startangle_c: 9 -endpose_c: 2 1 9 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0111 0.0056 3.5343 -0.0222 0.0111 3.5343 -0.0333 0.0167 3.5343 -0.0444 0.0222 3.5343 -0.0556 0.0278 3.5343 -0.0667 0.0333 3.5343 -0.0778 0.0389 3.5343 -0.0889 0.0444 3.5343 -0.1000 0.0500 3.5343 -primID: 3 -startangle_c: 9 -endpose_c: -5 -4 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0298 -0.0184 3.5646 --0.0592 -0.0379 3.5949 --0.0881 -0.0583 3.6252 --0.1165 -0.0796 3.6555 --0.1444 -0.1019 3.6858 --0.1717 -0.1251 3.7161 --0.1984 -0.1492 3.7464 --0.2245 -0.1742 3.7767 --0.2500 -0.2000 3.8070 -primID: 4 -startangle_c: 9 -endpose_c: -7 -2 8 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.5343 --0.0377 -0.0156 3.5343 --0.0754 -0.0312 3.5343 --0.1131 -0.0468 3.5343 --0.1508 -0.0623 3.5152 --0.1893 -0.0758 3.4405 --0.2287 -0.0863 3.3658 --0.2687 -0.0939 3.2910 --0.3092 -0.0985 3.2163 --0.3500 -0.1000 3.1416 -primID: 5 -startangle_c: 9 -endpose_c: 0 0 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 9 -endpose_c: 0 0 8 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.5343 -0.0000 0.0000 3.4907 -0.0000 0.0000 3.4470 -0.0000 0.0000 3.4034 -0.0000 0.0000 3.3598 -0.0000 0.0000 3.3161 -0.0000 0.0000 3.2725 -0.0000 0.0000 3.2289 -0.0000 0.0000 3.1852 -0.0000 0.0000 3.1416 -primID: 0 -startangle_c: 10 -endpose_c: -1 -1 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0056 -0.0056 3.9270 --0.0111 -0.0111 3.9270 --0.0167 -0.0167 3.9270 --0.0222 -0.0222 3.9270 --0.0278 -0.0278 3.9270 --0.0333 -0.0333 3.9270 --0.0389 -0.0389 3.9270 --0.0444 -0.0444 3.9270 --0.0500 -0.0500 3.9270 -primID: 1 -startangle_c: 10 -endpose_c: -6 -6 10 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0333 -0.0333 3.9270 --0.0667 -0.0667 3.9270 --0.1000 -0.1000 3.9270 --0.1333 -0.1333 3.9270 --0.1667 -0.1667 3.9270 --0.2000 -0.2000 3.9270 --0.2333 -0.2333 3.9270 --0.2667 -0.2667 3.9270 --0.3000 -0.3000 3.9270 -primID: 2 -startangle_c: 10 -endpose_c: 1 1 10 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0056 0.0056 3.9270 -0.0111 0.0111 3.9270 -0.0167 0.0167 3.9270 -0.0222 0.0222 3.9270 -0.0278 0.0278 3.9270 -0.0333 0.0333 3.9270 -0.0389 0.0389 3.9270 -0.0444 0.0444 3.9270 -0.0500 0.0500 3.9270 -primID: 3 -startangle_c: 10 -endpose_c: -5 -7 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0678 -0.0684 3.9567 --0.1000 -0.1043 4.0085 --0.1302 -0.1418 4.0604 --0.1584 -0.1809 4.1123 --0.1846 -0.2213 4.1641 --0.2086 -0.2631 4.2160 --0.2304 -0.3060 4.2678 --0.2500 -0.3500 4.3197 -primID: 4 -startangle_c: 10 -endpose_c: -7 -5 9 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 3.9270 --0.0341 -0.0341 3.9270 --0.0684 -0.0678 3.8973 --0.1043 -0.1000 3.8455 --0.1418 -0.1302 3.7936 --0.1809 -0.1584 3.7417 --0.2213 -0.1846 3.6899 --0.2631 -0.2086 3.6380 --0.3060 -0.2304 3.5862 --0.3500 -0.2500 3.5343 -primID: 5 -startangle_c: 10 -endpose_c: 0 0 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.9706 -0.0000 0.0000 4.0143 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.3197 -primID: 6 -startangle_c: 10 -endpose_c: 0 0 9 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 3.9270 -0.0000 0.0000 3.8834 -0.0000 0.0000 3.8397 -0.0000 0.0000 3.7961 -0.0000 0.0000 3.7525 -0.0000 0.0000 3.7088 -0.0000 0.0000 3.6652 -0.0000 0.0000 3.6216 -0.0000 0.0000 3.5779 -0.0000 0.0000 3.5343 -primID: 0 -startangle_c: 11 -endpose_c: -1 -2 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0056 -0.0111 4.3197 --0.0111 -0.0222 4.3197 --0.0167 -0.0333 4.3197 --0.0222 -0.0444 4.3197 --0.0278 -0.0556 4.3197 --0.0333 -0.0667 4.3197 --0.0389 -0.0778 4.3197 --0.0444 -0.0889 4.3197 --0.0500 -0.1000 4.3197 -primID: 1 -startangle_c: 11 -endpose_c: -3 -6 11 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0167 -0.0333 4.3197 --0.0333 -0.0667 4.3197 --0.0500 -0.1000 4.3197 --0.0667 -0.1333 4.3197 --0.0833 -0.1667 4.3197 --0.1000 -0.2000 4.3197 --0.1167 -0.2333 4.3197 --0.1333 -0.2667 4.3197 --0.1500 -0.3000 4.3197 -primID: 2 -startangle_c: 11 -endpose_c: 1 2 11 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0056 0.0111 4.3197 -0.0111 0.0222 4.3197 -0.0167 0.0333 4.3197 -0.0222 0.0444 4.3197 -0.0278 0.0556 4.3197 -0.0333 0.0667 4.3197 -0.0389 0.0778 4.3197 -0.0444 0.0889 4.3197 -0.0500 0.1000 4.3197 -primID: 3 -startangle_c: 11 -endpose_c: -4 -5 10 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0184 -0.0298 4.2894 --0.0379 -0.0592 4.2591 --0.0583 -0.0881 4.2288 --0.0796 -0.1165 4.1985 --0.1019 -0.1444 4.1682 --0.1251 -0.1717 4.1379 --0.1492 -0.1984 4.1076 --0.1742 -0.2245 4.0773 --0.2000 -0.2500 4.0470 -primID: 4 -startangle_c: 11 -endpose_c: -2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.3197 --0.0156 -0.0377 4.3197 --0.0312 -0.0754 4.3197 --0.0468 -0.1131 4.3197 --0.0623 -0.1508 4.3388 --0.0758 -0.1893 4.4135 --0.0863 -0.2287 4.4882 --0.0939 -0.2687 4.5629 --0.0985 -0.3092 4.6377 --0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 11 -endpose_c: 0 0 10 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.2761 -0.0000 0.0000 4.2324 -0.0000 0.0000 4.1888 -0.0000 0.0000 4.1452 -0.0000 0.0000 4.1015 -0.0000 0.0000 4.0579 -0.0000 0.0000 4.0143 -0.0000 0.0000 3.9706 -0.0000 0.0000 3.9270 -primID: 6 -startangle_c: 11 -endpose_c: 0 0 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.3197 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 12 -endpose_c: 0 -1 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0056 4.7124 -0.0000 -0.0111 4.7124 -0.0000 -0.0167 4.7124 -0.0000 -0.0222 4.7124 -0.0000 -0.0278 4.7124 -0.0000 -0.0333 4.7124 -0.0000 -0.0389 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0500 4.7124 -primID: 1 -startangle_c: 12 -endpose_c: 0 -8 12 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0444 4.7124 -0.0000 -0.0889 4.7124 -0.0000 -0.1333 4.7124 -0.0000 -0.1778 4.7124 -0.0000 -0.2222 4.7124 -0.0000 -0.2667 4.7124 -0.0000 -0.3111 4.7124 -0.0000 -0.3556 4.7124 -0.0000 -0.4000 4.7124 -primID: 2 -startangle_c: 12 -endpose_c: 0 1 12 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0056 4.7124 -0.0000 0.0111 4.7124 -0.0000 0.0167 4.7124 -0.0000 0.0222 4.7124 -0.0000 0.0278 4.7124 -0.0000 0.0333 4.7124 -0.0000 0.0389 4.7124 -0.0000 0.0444 4.7124 -0.0000 0.0500 4.7124 -primID: 3 -startangle_c: 12 -endpose_c: 1 -8 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 -0.0452 4.7124 -0.0000 -0.0904 4.7124 -0.0000 -0.1355 4.7124 -0.0008 -0.1807 4.7612 -0.0045 -0.2257 4.8300 -0.0114 -0.2703 4.8988 -0.0213 -0.3144 4.9675 -0.0342 -0.3577 5.0363 -0.0500 -0.4000 5.1051 -primID: 4 -startangle_c: 12 -endpose_c: -1 -8 11 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 4.7124 --0.0000 -0.0452 4.7124 --0.0000 -0.0904 4.7124 --0.0000 -0.1355 4.7124 --0.0008 -0.1807 4.6636 --0.0045 -0.2257 4.5948 --0.0114 -0.2703 4.5260 --0.0213 -0.3144 4.4572 --0.0342 -0.3577 4.3885 --0.0500 -0.4000 4.3197 -primID: 5 -startangle_c: 12 -endpose_c: 0 0 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.9742 -0.0000 0.0000 5.0178 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.1051 -primID: 6 -startangle_c: 12 -endpose_c: 0 0 11 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 4.7124 -0.0000 0.0000 4.6688 -0.0000 0.0000 4.6251 -0.0000 0.0000 4.5815 -0.0000 0.0000 4.5379 -0.0000 0.0000 4.4942 -0.0000 0.0000 4.4506 -0.0000 0.0000 4.4070 -0.0000 0.0000 4.3633 -0.0000 0.0000 4.3197 -primID: 0 -startangle_c: 13 -endpose_c: 1 -2 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0056 -0.0111 5.1051 -0.0111 -0.0222 5.1051 -0.0167 -0.0333 5.1051 -0.0222 -0.0444 5.1051 -0.0278 -0.0556 5.1051 -0.0333 -0.0667 5.1051 -0.0389 -0.0778 5.1051 -0.0444 -0.0889 5.1051 -0.0500 -0.1000 5.1051 -primID: 1 -startangle_c: 13 -endpose_c: 3 -6 13 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0167 -0.0333 5.1051 -0.0333 -0.0667 5.1051 -0.0500 -0.1000 5.1051 -0.0667 -0.1333 5.1051 -0.0833 -0.1667 5.1051 -0.1000 -0.2000 5.1051 -0.1167 -0.2333 5.1051 -0.1333 -0.2667 5.1051 -0.1500 -0.3000 5.1051 -primID: 2 -startangle_c: 13 -endpose_c: -1 2 13 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.1051 --0.0056 0.0111 5.1051 --0.0111 0.0222 5.1051 --0.0167 0.0333 5.1051 --0.0222 0.0444 5.1051 --0.0278 0.0556 5.1051 --0.0333 0.0667 5.1051 --0.0389 0.0778 5.1051 --0.0444 0.0889 5.1051 --0.0500 0.1000 5.1051 -primID: 3 -startangle_c: 13 -endpose_c: 4 -5 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0184 -0.0298 5.1354 -0.0379 -0.0592 5.1657 -0.0583 -0.0881 5.1960 -0.0796 -0.1165 5.2263 -0.1019 -0.1444 5.2566 -0.1251 -0.1717 5.2869 -0.1492 -0.1984 5.3172 -0.1742 -0.2245 5.3475 -0.2000 -0.2500 5.3778 -primID: 4 -startangle_c: 13 -endpose_c: 2 -7 12 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0156 -0.0377 5.1051 -0.0312 -0.0754 5.1051 -0.0468 -0.1131 5.1051 -0.0623 -0.1508 5.0860 -0.0758 -0.1893 5.0113 -0.0863 -0.2287 4.9366 -0.0939 -0.2687 4.8618 -0.0985 -0.3092 4.7871 -0.1000 -0.3500 4.7124 -primID: 5 -startangle_c: 13 -endpose_c: 0 0 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 13 -endpose_c: 0 0 12 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.1051 -0.0000 0.0000 5.0615 -0.0000 0.0000 5.0178 -0.0000 0.0000 4.9742 -0.0000 0.0000 4.9306 -0.0000 0.0000 4.8869 -0.0000 0.0000 4.8433 -0.0000 0.0000 4.7997 -0.0000 0.0000 4.7560 -0.0000 0.0000 4.7124 -primID: 0 -startangle_c: 14 -endpose_c: 1 -1 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0056 -0.0056 5.4978 -0.0111 -0.0111 5.4978 -0.0167 -0.0167 5.4978 -0.0222 -0.0222 5.4978 -0.0278 -0.0278 5.4978 -0.0333 -0.0333 5.4978 -0.0389 -0.0389 5.4978 -0.0444 -0.0444 5.4978 -0.0500 -0.0500 5.4978 -primID: 1 -startangle_c: 14 -endpose_c: 6 -6 14 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0333 -0.0333 5.4978 -0.0667 -0.0667 5.4978 -0.1000 -0.1000 5.4978 -0.1333 -0.1333 5.4978 -0.1667 -0.1667 5.4978 -0.2000 -0.2000 5.4978 -0.2333 -0.2333 5.4978 -0.2667 -0.2667 5.4978 -0.3000 -0.3000 5.4978 -primID: 2 -startangle_c: 14 -endpose_c: -1 1 14 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.4978 --0.0056 0.0056 5.4978 --0.0111 0.0111 5.4978 --0.0167 0.0167 5.4978 --0.0222 0.0222 5.4978 --0.0278 0.0278 5.4978 --0.0333 0.0333 5.4978 --0.0389 0.0389 5.4978 --0.0444 0.0444 5.4978 --0.0500 0.0500 5.4978 -primID: 3 -startangle_c: 14 -endpose_c: 7 -5 15 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0684 -0.0678 5.5275 -0.1043 -0.1000 5.5793 -0.1418 -0.1302 5.6312 -0.1809 -0.1584 5.6830 -0.2213 -0.1846 5.7349 -0.2631 -0.2086 5.7868 -0.3060 -0.2304 5.8386 -0.3500 -0.2500 5.8905 -primID: 4 -startangle_c: 14 -endpose_c: 5 -7 13 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0341 -0.0341 5.4978 -0.0678 -0.0684 5.4681 -0.1000 -0.1043 5.4162 -0.1302 -0.1418 5.3644 -0.1584 -0.1809 5.3125 -0.1846 -0.2213 5.2607 -0.2086 -0.2631 5.2088 -0.2304 -0.3060 5.1569 -0.2500 -0.3500 5.1051 -primID: 5 -startangle_c: 14 -endpose_c: 0 0 15 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8905 -primID: 6 -startangle_c: 14 -endpose_c: 0 0 13 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.4978 -0.0000 0.0000 5.4542 -0.0000 0.0000 5.4105 -0.0000 0.0000 5.3669 -0.0000 0.0000 5.3233 -0.0000 0.0000 5.2796 -0.0000 0.0000 5.2360 -0.0000 0.0000 5.1924 -0.0000 0.0000 5.1487 -0.0000 0.0000 5.1051 -primID: 0 -startangle_c: 15 -endpose_c: 2 -1 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0111 -0.0056 5.8905 -0.0222 -0.0111 5.8905 -0.0333 -0.0167 5.8905 -0.0444 -0.0222 5.8905 -0.0556 -0.0278 5.8905 -0.0667 -0.0333 5.8905 -0.0778 -0.0389 5.8905 -0.0889 -0.0444 5.8905 -0.1000 -0.0500 5.8905 -primID: 1 -startangle_c: 15 -endpose_c: 6 -3 15 -additionalactioncostmult: 1 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0333 -0.0167 5.8905 -0.0667 -0.0333 5.8905 -0.1000 -0.0500 5.8905 -0.1333 -0.0667 5.8905 -0.1667 -0.0833 5.8905 -0.2000 -0.1000 5.8905 -0.2333 -0.1167 5.8905 -0.2667 -0.1333 5.8905 -0.3000 -0.1500 5.8905 -primID: 2 -startangle_c: 15 -endpose_c: -2 1 15 -additionalactioncostmult: 40 -intermediateposes: 10 -0.0000 0.0000 5.8905 --0.0111 0.0056 5.8905 --0.0222 0.0111 5.8905 --0.0333 0.0167 5.8905 --0.0444 0.0222 5.8905 --0.0556 0.0278 5.8905 --0.0667 0.0333 5.8905 --0.0778 0.0389 5.8905 --0.0889 0.0444 5.8905 --0.1000 0.0500 5.8905 -primID: 3 -startangle_c: 15 -endpose_c: 5 -4 14 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0298 -0.0184 5.8602 -0.0592 -0.0379 5.8299 -0.0881 -0.0583 5.7996 -0.1165 -0.0796 5.7693 -0.1444 -0.1019 5.7390 -0.1717 -0.1251 5.7087 -0.1984 -0.1492 5.6784 -0.2245 -0.1742 5.6481 -0.2500 -0.2000 5.6178 -primID: 4 -startangle_c: 15 -endpose_c: 7 -2 0 -additionalactioncostmult: 2 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0377 -0.0156 5.8905 -0.0754 -0.0312 5.8905 -0.1131 -0.0468 5.8905 -0.1508 -0.0623 5.9096 -0.1893 -0.0758 5.9843 -0.2287 -0.0863 6.0590 -0.2687 -0.0939 6.1337 -0.3092 -0.0985 6.2085 -0.3500 -0.1000 6.2832 -primID: 5 -startangle_c: 15 -endpose_c: 0 0 14 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.8469 -0.0000 0.0000 5.8032 -0.0000 0.0000 5.7596 -0.0000 0.0000 5.7160 -0.0000 0.0000 5.6723 -0.0000 0.0000 5.6287 -0.0000 0.0000 5.5851 -0.0000 0.0000 5.5414 -0.0000 0.0000 5.4978 -primID: 6 -startangle_c: 15 -endpose_c: 0 0 0 -additionalactioncostmult: 20 -intermediateposes: 10 -0.0000 0.0000 5.8905 -0.0000 0.0000 5.9341 -0.0000 0.0000 5.9778 -0.0000 0.0000 6.0214 -0.0000 0.0000 6.0650 -0.0000 0.0000 6.1087 -0.0000 0.0000 6.1523 -0.0000 0.0000 6.1959 -0.0000 0.0000 6.2396 -0.0000 0.0000 0.0000 diff --git a/Controllers/Packages/amr_startup/config/mqtt_general.yaml b/Controllers/Packages/amr_startup/config/mqtt_general.yaml deleted file mode 100644 index 2f172c9..0000000 --- a/Controllers/Packages/amr_startup/config/mqtt_general.yaml +++ /dev/null @@ -1,8 +0,0 @@ -MQTT: - Name: T801 - Host: 172.20.235.170 - Port: 1885 - Client_ID: T801 - Username: robotics - Password: robotics - Keep_Alive: 60 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml deleted file mode 100644 index e0431be..0000000 --- a/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml +++ /dev/null @@ -1,188 +0,0 @@ - -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) \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml b/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml deleted file mode 100755 index cf1a0da..0000000 --- a/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml +++ /dev/null @@ -1,71 +0,0 @@ -# ----------------------------------- -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 diff --git a/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml b/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml deleted file mode 100755 index ec2ac75..0000000 --- a/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml +++ /dev/null @@ -1,10 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml b/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml deleted file mode 100644 index 959a137..0000000 --- a/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml +++ /dev/null @@ -1,61 +0,0 @@ -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 - # \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/two_points_global_params.yaml b/Controllers/Packages/amr_startup/config/two_points_global_params.yaml deleted file mode 100644 index ca0b60f..0000000 --- a/Controllers/Packages/amr_startup/config/two_points_global_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -base_global_planner: TwoPointsPlanner -TwoPointsPlanner: - lethal_obstacle: 20 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/amr_control.launch b/Controllers/Packages/amr_startup/launch/amr_control.launch deleted file mode 100644 index 269a45a..0000000 --- a/Controllers/Packages/amr_startup/launch/amr_control.launch +++ /dev/null @@ -1,50 +0,0 @@ - - - - - - - - - - - - - - - [[0.412, -0.304], [0.412, 0.304], [-0.412, 0.304], [-0.412, -0.304]] - - - - [[0.511,-0.1955],[0.511,0.1955],[-0.511,0.1955],[-0.511,-0.1955]] - - - - [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/amr_startup.launch b/Controllers/Packages/amr_startup/launch/amr_startup.launch deleted file mode 100644 index 5a53679..0000000 --- a/Controllers/Packages/amr_startup/launch/amr_startup.launch +++ /dev/null @@ -1,102 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch b/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch deleted file mode 100755 index 51daad5..0000000 --- a/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/Controllers/Packages/amr_startup/launch/hector_mapping.launch b/Controllers/Packages/amr_startup/launch/hector_mapping.launch deleted file mode 100644 index d74fa3e..0000000 --- a/Controllers/Packages/amr_startup/launch/hector_mapping.launch +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/includes/amcl.launch b/Controllers/Packages/amr_startup/launch/includes/amcl.launch deleted file mode 100644 index 3569a2c..0000000 --- a/Controllers/Packages/amr_startup/launch/includes/amcl.launch +++ /dev/null @@ -1,105 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml b/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml deleted file mode 100644 index 1b77cca..0000000 --- a/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/includes/rviz.launch b/Controllers/Packages/amr_startup/launch/includes/rviz.launch deleted file mode 100644 index b415953..0000000 --- a/Controllers/Packages/amr_startup/launch/includes/rviz.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml b/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml deleted file mode 100644 index 0c0063a..0000000 --- a/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch deleted file mode 100644 index b223b41..0000000 --- a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/robot_empty_world.launch b/Controllers/Packages/amr_startup/launch/robot_empty_world.launch deleted file mode 100644 index af3ae07..0000000 --- a/Controllers/Packages/amr_startup/launch/robot_empty_world.launch +++ /dev/null @@ -1,72 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch b/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch deleted file mode 100644 index 3efd0c4..0000000 --- a/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch +++ /dev/null @@ -1,59 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - [robot/joint_states] - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/robot_maze_world.launch b/Controllers/Packages/amr_startup/launch/robot_maze_world.launch deleted file mode 100644 index 7569152..0000000 --- a/Controllers/Packages/amr_startup/launch/robot_maze_world.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/sehc_world.launch b/Controllers/Packages/amr_startup/launch/sehc_world.launch deleted file mode 100644 index 1e35cc4..0000000 --- a/Controllers/Packages/amr_startup/launch/sehc_world.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/launch/start_maps.launch b/Controllers/Packages/amr_startup/launch/start_maps.launch deleted file mode 100644 index aacc94c..0000000 --- a/Controllers/Packages/amr_startup/launch/start_maps.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - diff --git a/Controllers/Packages/amr_startup/launch/ware_houses_world.launch b/Controllers/Packages/amr_startup/launch/ware_houses_world.launch deleted file mode 100644 index b05446f..0000000 --- a/Controllers/Packages/amr_startup/launch/ware_houses_world.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/package.xml b/Controllers/Packages/amr_startup/package.xml deleted file mode 100644 index 0413dec..0000000 --- a/Controllers/Packages/amr_startup/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - amr_startup - 0.0.0 - The amr_startup package - - - - - robotics - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - diff --git a/Controllers/Packages/amr_startup/rosconsole.config b/Controllers/Packages/amr_startup/rosconsole.config deleted file mode 100644 index 7c0221b..0000000 --- a/Controllers/Packages/amr_startup/rosconsole.config +++ /dev/null @@ -1,2 +0,0 @@ -log4j.logger.ros.tf=ERROR -log4j.logger.ros.tf2=ERROR \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz deleted file mode 100644 index 1f16975..0000000 --- a/Controllers/Packages/amr_startup/rviz/navigation.rviz +++ /dev/null @@ -1,646 +0,0 @@ -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 diff --git a/Controllers/Packages/amr_startup/sdf/maze/model.config b/Controllers/Packages/amr_startup/sdf/maze/model.config deleted file mode 100644 index 9a00866..0000000 --- a/Controllers/Packages/amr_startup/sdf/maze/model.config +++ /dev/null @@ -1,11 +0,0 @@ - - - maze - 1.0 - model.sdf - - Martin Günther - martin.guenther@dfki.de - - - diff --git a/Controllers/Packages/amr_startup/sdf/maze/model.sdf b/Controllers/Packages/amr_startup/sdf/maze/model.sdf deleted file mode 100644 index b31cc0b..0000000 --- a/Controllers/Packages/amr_startup/sdf/maze/model.sdf +++ /dev/null @@ -1,345 +0,0 @@ - - - - -0.078283 0.098984 0 0 -0 0 - - - - - 20 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 20 0.15 2.5 - - - - - 1 1 1 1 - - - 0.030536 9.925 0 0 -0 0 - - - - - - 20 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 20 0.15 2.5 - - - - - 1 1 1 1 - - - 9.95554 0 0 0 0 -1.5708 - - - - - - 20 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 20 0.15 2.5 - - - - - 1 1 1 1 - - - 0.030536 -9.925 0 0 -0 3.14159 - - - - - - 1.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 1.5 0.15 2.5 - - - - - 1 1 1 1 - - - 5.35089 3.21906 0 0 -0 3.14159 - - - - - - 5.25 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 5.25 0.15 2.5 - - - - - 1 1 1 1 - - - 4.67589 5.76906 0 0 -0 1.5708 - - - - - - 5.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 5.5 0.15 2.5 - - - - - 1 1 1 1 - - - 7.10914 4.73454 0 0 0 -1.5708 - - - - - - 3 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 3 0.15 2.5 - - - - - 1 1 1 1 - - - 8.53414 2.05954 0 0 -0 0 - - - - - - 20 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 20 0.15 2.5 - - - - - 1 1 1 1 - - - -9.89446 0 0 0 -0 1.5708 - - - - - - 5.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 5.5 0.15 2.5 - - - - - 1 1 1 1 - - - -4.35914 -2.82889 0 0 0 -1.5708 - - - - - - 5.75 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 5.75 0.15 2.5 - - - - - 1 1 1 1 - - - -7.15914 -5.50389 0 0 -0 3.14159 - - - - - - 16 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 16 0.15 2.5 - - - - - 1 1 1 1 - - - -1.89911 1.86906 0 0 -0 0 - - - - - - 1.5 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 1.5 0.15 2.5 - - - - - 1 1 1 1 - - - 6.02589 2.54406 0 0 -0 1.5708 - - - - - - 0.15 0.15 2.5 - - - 0 0 1.25 0 -0 0 - - - 0 0 1.25 0 -0 0 - - - 0.15 0.15 2.5 - - - - - 1 1 1 1 - - - 6.02589 3.21906 0 0 -0 0 - - 1 - - diff --git a/Devices/Packages/diff_wheel_plugin/CMakeLists.txt b/Devices/Packages/diff_wheel_plugin/CMakeLists.txt index 6462a7d..6bfe5c4 100755 --- a/Devices/Packages/diff_wheel_plugin/CMakeLists.txt +++ b/Devices/Packages/diff_wheel_plugin/CMakeLists.txt @@ -137,6 +137,13 @@ add_library(${PROJECT_NAME} ## 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}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(${PROJECT_NAME} PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context @@ -154,6 +161,18 @@ add_executable(diff_wheel_controller src/diff_wheel_controller.cpp) ## same as for the library above add_dependencies(diff_wheel_feedback ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(diff_wheel_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(diff_wheel_feedback PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) +set_target_properties(diff_wheel_controller PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) ## Specify libraries to link a library or executable target against target_link_libraries(${PROJECT_NAME} diff --git a/Localizations/Cores/loc_core/include/loc_core/common.h b/Localizations/Cores/loc_core/include/loc_core/common.h deleted file mode 100644 index 90f816c..0000000 --- a/Localizations/Cores/loc_core/include/loc_core/common.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef _LOC_CORE_COMMON_H_INCLUDED_ -#define _LOC_CORE_COMMON_H_INCLUDED_ - -#include -#include - -using TFListenerPtr = std::shared_ptr; - -#endif // _LOC_CORE_COMMON_H_INCLUDED_ diff --git a/Localizations/Cores/loc_core/include/loc_core/localization.h b/Localizations/Cores/loc_core/include/loc_core/localization.h index 08ef05a..500a27d 100644 --- a/Localizations/Cores/loc_core/include/loc_core/localization.h +++ b/Localizations/Cores/loc_core/include/loc_core/localization.h @@ -6,7 +6,7 @@ #include #include #include -#include +// #include namespace loc_core { @@ -39,7 +39,7 @@ namespace loc_core * @param nh A Node handle * */ - virtual void initialize(ros::NodeHandle nh, TFListenerPtr tf) = 0; + virtual void initialize(ros::NodeHandle nh, std::shared_ptr tf) = 0; /** * @brief Loading a Activate Map File name diff --git a/Localizations/Libraries/Ros/map_server/CMakeLists.txt b/Localizations/Libraries/Ros/map_server/CMakeLists.txt index f5c7ceb..cf2593b 100644 --- a/Localizations/Libraries/Ros/map_server/CMakeLists.txt +++ b/Localizations/Libraries/Ros/map_server/CMakeLists.txt @@ -1,6 +1,13 @@ cmake_minimum_required(VERSION 3.0.2) project(map_server) +# Suppress RPATH cycle warnings by disabling automatic RPATH detection +# We'll set RPATH explicitly for each target +set(CMAKE_SKIP_RPATH FALSE) +set(CMAKE_SKIP_BUILD_RPATH FALSE) +set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +set(CMAKE_INSTALL_RPATH_USE_LINK_PATH FALSE) + find_package(catkin REQUIRED COMPONENTS roscpp @@ -56,6 +63,13 @@ target_link_libraries(map_server_image_loader ${SDL_IMAGE_LIBRARIES} ${YAMLCPP_LIBRARIES} ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(map_server_image_loader PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_library(map_server_lib src/map_server.cpp) add_dependencies(map_server_lib map_server_image_loader ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -67,6 +81,13 @@ target_link_libraries(map_server_lib ${SDL_IMAGE_LIBRARIES} ${YAMLCPP_LIBRARIES} ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(map_server_lib PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_library (map_generator_lib src/map_generator.cpp) add_dependencies(map_generator_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -85,6 +106,13 @@ target_link_libraries(map_server ${YAMLCPP_LIBRARIES} ${catkin_LIBRARIES} ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(map_server PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_executable(map_server-map_saver src/map_saver.cpp) add_dependencies(map_server-map_saver map_generator_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -114,6 +142,13 @@ if(CATKIN_ENABLE_TESTING) ${SDL_LIBRARY} ${SDL_IMAGE_LIBRARIES} ) + # Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings + # Libraries will be found via LD_LIBRARY_PATH or system paths + set_target_properties(${PROJECT_NAME}_utest PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE + ) find_package(roslib REQUIRED) include_directories(${roslib_INCLUDE_DIRS}) diff --git a/Localizations/Packages/loc_base/CMakeLists.txt b/Localizations/Packages/loc_base/CMakeLists.txt index 11ebdf1..8dc92f4 100644 --- a/Localizations/Packages/loc_base/CMakeLists.txt +++ b/Localizations/Packages/loc_base/CMakeLists.txt @@ -154,6 +154,13 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} yaml-cpp ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(${PROJECT_NAME} PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) ## 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 @@ -173,6 +180,13 @@ add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catk target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(${PROJECT_NAME}_node PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) ############# ## Install ## diff --git a/Localizations/Packages/loc_base/include/loc_base/loc_base.h b/Localizations/Packages/loc_base/include/loc_base/loc_base.h index 2acf08a..b38e9e6 100644 --- a/Localizations/Packages/loc_base/include/loc_base/loc_base.h +++ b/Localizations/Packages/loc_base/include/loc_base/loc_base.h @@ -29,7 +29,7 @@ namespace loc_base * @param nh A Node handle * @param tf A poiter tranform listen */ - void initialize(ros::NodeHandle nh, TFListenerPtr tf) override; + void initialize(ros::NodeHandle nh, std::shared_ptr tf) override; /** * @brief Loading a Activate Map File name @@ -147,7 +147,7 @@ namespace loc_base ros::NodeHandle nh_; ros::NodeHandle private_nh_; ros::Publisher init_pub_; - static TFListenerPtr tf_; + static std::shared_ptr tf_; XmlRpc::XmlRpcValue plugins_; // std::map map_file_vt_; diff --git a/Localizations/Packages/loc_base/src/loc_base.cpp b/Localizations/Packages/loc_base/src/loc_base.cpp index 2fb483f..fb4e05f 100644 --- a/Localizations/Packages/loc_base/src/loc_base.cpp +++ b/Localizations/Packages/loc_base/src/loc_base.cpp @@ -8,7 +8,7 @@ std::shared_ptr loc_base::LocBase::localization_althm_ = nullptr; std::shared_ptr loc_base::LocBase::mapping_althm_ = nullptr; -TFListenerPtr loc_base::LocBase::tf_; +std::shared_ptr loc_base::LocBase::tf_; std::string loc_base::LocBase::base_frame_id_; std::string loc_base::LocBase::global_frame_id_; std::string* loc_base::LocBase::working_dir_ptr_ = NULL; @@ -38,7 +38,7 @@ void loc_base::LocBase::cleanup() { loc_base::LocBase::mapping_althm_.reset(); } -void loc_base::LocBase::initialize(ros::NodeHandle nh, TFListenerPtr tf) +void loc_base::LocBase::initialize(ros::NodeHandle nh, std::shared_ptr tf) { if (!initialized_) { diff --git a/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt b/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt index 930f717..eb3a02d 100644 --- a/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt +++ b/Localizations/Packages/slam_toolbox/slam_toolbox/CMakeLists.txt @@ -98,39 +98,116 @@ target_link_libraries(ceres_solver_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TBB_LIBRARIES} ) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(ceres_solver_plugin PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) #### Tool lib for mapping add_library(toolbox_common src/slam_toolbox_common.cpp src/map_saver.cpp src/loop_closure_assistant.cpp src/laser_utils.cpp src/slam_mapper.cpp) target_link_libraries(toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(toolbox_common PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) #### Mapping executibles add_library(async_slam_toolbox src/slam_toolbox_async.cpp) add_dependencies(async_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(async_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(async_slam_toolbox PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_executable(async_slam_toolbox_node src/slam_toolbox_async_node.cpp ) target_link_libraries(async_slam_toolbox_node async_slam_toolbox) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(async_slam_toolbox_node PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_library(sync_slam_toolbox src/slam_toolbox_sync.cpp) add_dependencies(sync_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(sync_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(sync_slam_toolbox PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_executable(sync_slam_toolbox_node src/slam_toolbox_sync_node.cpp ) target_link_libraries(sync_slam_toolbox_node sync_slam_toolbox) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(sync_slam_toolbox_node PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_library(localization_slam_toolbox src/slam_toolbox_localization.cpp) add_dependencies(localization_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(localization_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(localization_slam_toolbox PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_executable(localization_slam_toolbox_node src/slam_toolbox_localization_node.cpp ) target_link_libraries(localization_slam_toolbox_node localization_slam_toolbox) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(localization_slam_toolbox_node PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_library(lifelong_slam_toolbox src/experimental/slam_toolbox_lifelong.cpp) add_dependencies(lifelong_slam_toolbox ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(lifelong_slam_toolbox toolbox_common kartoSlamToolbox ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(lifelong_slam_toolbox PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) add_executable(lifelong_slam_toolbox_node src/experimental/slam_toolbox_lifelong_node.cpp ) target_link_libraries(lifelong_slam_toolbox_node lifelong_slam_toolbox) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(lifelong_slam_toolbox_node PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) #### Merging maps tool add_executable(merge_maps_kinematic src/merge_maps_kinematic.cpp) target_link_libraries(merge_maps_kinematic toolbox_common) +# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings +# Libraries will be found via LD_LIBRARY_PATH or system paths +set_target_properties(merge_maps_kinematic PROPERTIES + SKIP_BUILD_RPATH TRUE + BUILD_WITH_INSTALL_RPATH FALSE + INSTALL_RPATH_USE_LINK_PATH FALSE +) #### testing #if(CATKIN_ENABLE_TESTING) diff --git a/pnkx_nav_core b/pnkx_nav_core index 1de5a35..82804ff 160000 --- a/pnkx_nav_core +++ b/pnkx_nav_core @@ -1 +1 @@ -Subproject commit 1de5a35cc7985bc9890e8de2542f85d8ab90e2d7 +Subproject commit 82804ff93a35c060914c6212955ef996a9bda6da