diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt index 3cbebd6..509a440 100644 --- a/Controllers/Packages/amr_control/CMakeLists.txt +++ b/Controllers/Packages/amr_control/CMakeLists.txt @@ -18,6 +18,8 @@ find_package(catkin REQUIRED COMPONENTS loc_core amr_comunication vda5050_msgs + + robot_nav_2d_utils move_base_core robot_cpp ) @@ -26,83 +28,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(Boost REQUIRED COMPONENTS system thread) find_package(PkgConfig) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# geometry_msgs# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - - ################################### ## catkin specific configuration ## ################################### @@ -115,7 +40,14 @@ find_package(PkgConfig) catkin_package( INCLUDE_DIRS include LIBRARIES amr_control - CATKIN_DEPENDS geometry_msgs loc_core nav_2d_utils roscpp std_msgs move_base_core robot_cpp + CATKIN_DEPENDS + geometry_msgs + loc_core + nav_2d_utils + move_base_core + robot_cpp + roscpp + std_msgs DEPENDS Boost ) @@ -137,14 +69,15 @@ 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 @@ -156,29 +89,30 @@ set_target_properties(${PROJECT_NAME} PROPERTIES ## 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(${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 ## 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 ${PROJECT_NAME}) +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) ## 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 -# ) +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} @@ -200,11 +134,11 @@ 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} @@ -229,9 +163,9 @@ target_link_libraries(${PROJECT_NAME} ## 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 + 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/package.xml b/Controllers/Packages/amr_control/package.xml index 3287504..9e8c63c 100644 --- a/Controllers/Packages/amr_control/package.xml +++ b/Controllers/Packages/amr_control/package.xml @@ -63,9 +63,6 @@ loc_core amr_comunication vda5050_msgs - robot_cpp - move_base_core - geometry_msgs nav_2d_utils std_msgs @@ -75,9 +72,6 @@ loc_core amr_comunication vda5050_msgs - robot_cpp - move_base_core - geometry_msgs nav_2d_utils std_msgs @@ -87,7 +81,18 @@ loc_core amr_comunication vda5050_msgs + + + robot_cpp + robot_nav_2d_utils + move_base_core + + robot_cpp + robot_nav_2d_utils + move_base_core + robot_cpp + robot_nav_2d_utils move_base_core diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp index 0199ff6..314fbb0 100644 --- a/Controllers/Packages/amr_control/src/amr_control.cpp +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -233,23 +233,23 @@ namespace amr_control move_base_ptr_ = move_base_loader_(); // move_base_ptr_->initialize(tf_core_ptr_); - // ros::Rate r(3); - // do - // { - // r.sleep(); - // ros::spinOnce(); - // } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready); + ros::Rate r(3); + do + { + r.sleep(); + ros::spinOnce(); + } while (ros::ok() && !move_base_ptr_->getFeedback()->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); - // } + if (move_base_ptr_ != nullptr && + move_base_ptr_->getFeedback() != nullptr && + move_base_ptr_->getFeedback()->is_ready) + { + robot_geometry_msgs::Vector3 linear; + linear.x = 0.3; + move_base_ptr_->setTwistLinear(linear); + linear.x = -0.3; + move_base_ptr_->setTwistLinear(linear); + } } catch (const std::exception &e) { @@ -263,325 +263,325 @@ namespace amr_control } } - // 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_->getFeedback() != nullptr && + move_base_ptr_->getFeedback()->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_->getFeedback() != nullptr && + move_base_ptr_->getFeedback()->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; + robot_geometry_msgs::Vector3 linear; + robot_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 c3f2cf7..370818f 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 @@ -6,6 +6,12 @@ #include #include #include + +// pnkx core +#include +#include +#include + #include "amr_control/amr_opc_ua_server_api.h" #include "nova5_control/imr_nova_control.h" @@ -155,3298 +161,3307 @@ 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) + { + robot_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]; + + robot_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 robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", robot::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; + + robot_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]; + + robot_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 robot_geometry_msgs::PoseStamped goal = robot_nav_2d_utils::pose2DToPoseStamped(goal_2d, "map", robot::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; + + robot_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) + { + robot_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) + { + robot_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); + robot_geometry_msgs::PoseStamped goal = robot::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; + + robot_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_->getFeedback() != nullptr) + { + switch (amr_control::OpcUAServerAPI::move_base_ptr_->getFeedback()->navigation_state) + { + case robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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 robot::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_->getFeedback()->navigation_state == robot::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_->getFeedback()->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_control/src/amr_vda_5050_client_api.cpp b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp index bc62625..cc98291 100644 --- a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp +++ b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp @@ -20,7 +20,7 @@ namespace amr_control this->count_ok_max_ = new unsigned int(1); } - VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr move_base, + VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr move_base, std::shared_ptr loc_base, std::shared_ptr monitor) { VDA5050ClientAPI::move_base_ptr_ = move_base; @@ -116,7 +116,7 @@ namespace amr_control void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message) { - geometry_msgs::PoseStamped goal; + robot_geometry_msgs::PoseStamped goal; if (VDA5050ClientAPI::move_base_ptr_) { if (global_plan_msg_type_ == std::string("nav_msgs::Path")) @@ -150,9 +150,9 @@ namespace amr_control p_2d.theta = p.getYaw(); poses.push_back(p_2d); } - const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); - goal = path.poses.back(); - VDA5050ClientAPI::plan_pub_.publish(path); + // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); + // goal = path.poses.back(); + // VDA5050ClientAPI::plan_pub_.publish(path); } else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) { @@ -222,13 +222,13 @@ namespace amr_control VDA5050ClientAPI::plan_pub_.publish(order_msg); vda_5050::NodePosition position = order.nodes.back().nodePosition; - goal.header.stamp = ros::Time::now(); + goal.header.stamp = robot::Time::now(); goal.header.frame_id = "map"; goal.pose.position.x = position.x; goal.pose.position.y = position.y; - tf::Quaternion quat; - quat.setRPY(0.0, 0.0, position.theta); - tf::quaternionTFToMsg(quat, goal.pose.orientation); + // tf3::Transform transform; + // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta)); + // goal = tf3::toMsg(transform); } else return; @@ -239,13 +239,13 @@ namespace amr_control cmd_vel_max_.theta = 0.5; cmd_vel_max_saved_ = cmd_vel_max_; - geometry_msgs::Vector3 linear; + robot_geometry_msgs::Vector3 linear; linear.x = cmd_vel_max_.x; VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); linear.x = -cmd_vel_max_.x; VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); - geometry_msgs::Vector3 angular; + robot_geometry_msgs::Vector3 angular; angular.z = cmd_vel_max_.theta; VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); @@ -254,7 +254,7 @@ namespace amr_control void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message) { - geometry_msgs::PoseStamped goal; + robot_geometry_msgs::PoseStamped goal; if (VDA5050ClientAPI::move_base_ptr_) { if (global_plan_msg_type_ == std::string("nav_msgs::Path")) @@ -288,9 +288,9 @@ namespace amr_control p_2d.theta = p.getYaw(); poses.push_back(p_2d); } - const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); - goal = path.poses.back(); - VDA5050ClientAPI::plan_pub_.publish(path); + // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now()); + // goal = path.poses.back(); + // VDA5050ClientAPI::plan_pub_.publish(path); } else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) { @@ -359,13 +359,13 @@ namespace amr_control VDA5050ClientAPI::plan_pub_.publish(order_msg); vda_5050::NodePosition position = order.nodes.back().nodePosition; - goal.header.stamp = ros::Time::now(); + goal.header.stamp = robot::Time::now(); goal.header.frame_id = "map"; goal.pose.position.x = position.x; goal.pose.position.y = position.y; - tf::Quaternion quat; - quat.setRPY(0.0, 0.0, position.theta); - tf::quaternionTFToMsg(quat, goal.pose.orientation); + // tf3::Transform transform; + // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta)); + // goal = tf3::toMsg(transform); } else return; @@ -376,13 +376,13 @@ namespace amr_control cmd_vel_max_.theta = 0.5; cmd_vel_max_saved_ = cmd_vel_max_; - geometry_msgs::Vector3 linear; + robot_geometry_msgs::Vector3 linear; linear.x = cmd_vel_max_.x; VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); linear.x = -cmd_vel_max_.x; VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); - geometry_msgs::Vector3 angular; + robot_geometry_msgs::Vector3 angular; angular.z = cmd_vel_max_.theta; VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); @@ -392,19 +392,18 @@ namespace amr_control void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message) { - geometry_msgs::PoseStamped goal; - + robot_geometry_msgs::PoseStamped goal; if (VDA5050ClientAPI::move_base_ptr_) { if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal)) { vda_5050::NodePosition position = order.nodes.back().nodePosition; - goal.header.stamp = ros::Time::now(); + goal.header.stamp = robot::Time::now(); goal.header.frame_id = "map"; - tf::Quaternion quat; - quat.setRPY(0.0, 0.0, position.theta); - tf::quaternionTFToMsg(quat, goal.pose.orientation); - VDA5050ClientAPI::move_base_ptr_->rotateTo(goal); + // tf3::Transform transform; + // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta)); + // goal = tf3::toMsg(transform); + // VDA5050ClientAPI::move_base_ptr_->rotateTo(goal); } else return; @@ -521,10 +520,10 @@ namespace amr_control void VDA5050ClientAPI::updateVelocity(double velocity) { - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - if (nav_state == move_base_core::State::CONTROLLING) + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + if (nav_state == robot::move_base_core::State::CONTROLLING) { if (fabs(velocity) > 2.0) velocity = (fabs(velocity) / velocity) * 2.0; @@ -541,10 +540,10 @@ namespace amr_control void VDA5050ClientAPI::updateAngular(double angular) { - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - if (nav_state == move_base_core::State::CONTROLLING) + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + if (nav_state == robot::move_base_core::State::CONTROLLING) { if (fabs(angular) > 1.0) angular = (fabs(angular) / angular) * 1.0; @@ -645,7 +644,7 @@ namespace amr_control // Can runing actions be interrupted ? if (VDA5050ClientAPI::move_base_ptr_ != nullptr && - VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) + VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { VDA5050ClientAPI::move_base_ptr_->cancel(); *this->cancel_action_ = true; @@ -653,7 +652,7 @@ namespace amr_control *this->pause_action_ = false; ros::Rate r(5); while (ros::ok() && - VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING) + VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING) { ROS_INFO_THROTTLE(1.0, "Waiting to cancel"); r.sleep(); @@ -714,18 +713,18 @@ namespace amr_control void VDA5050ClientAPI::unDockFromStation(std::shared_ptr action, std::shared_ptr action_state) { - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; ros::Rate r(5); - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - while (ros::ok() && !*this->cancel_action_ && nav_state == move_base_core::State::CONTROLLING) + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + while (ros::ok() && !*this->cancel_action_ && nav_state == robot::move_base_core::State::CONTROLLING) { r.sleep(); ros::spinOnce(); } - geometry_msgs::PoseStamped pose; + robot_geometry_msgs::PoseStamped pose; if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose)) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; @@ -808,9 +807,9 @@ namespace amr_control return; } - geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance); + robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance); VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal); - geometry_msgs::Vector3 linear; + robot_geometry_msgs::Vector3 linear; linear.x = fabs(velocity); VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear); linear.x = -fabs(velocity); @@ -818,7 +817,7 @@ namespace amr_control while (ros::ok() && !*this->cancel_action_) { - if (nav_state == move_base_core::State::CONTROLLING) + if (nav_state == robot::move_base_core::State::CONTROLLING) break; r.sleep(); ros::spinOnce(); @@ -827,15 +826,15 @@ namespace amr_control while (ros::ok() && !*this->cancel_action_) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; - if (nav_state == move_base_core::State::SUCCEEDED || - nav_state == move_base_core::State::ABORTED || - nav_state == move_base_core::State::PREEMPTED) + if (nav_state == robot::move_base_core::State::SUCCEEDED || + nav_state == robot::move_base_core::State::ABORTED || + nav_state == robot::move_base_core::State::PREEMPTED) break; r.sleep(); ros::spinOnce(); } - if (nav_state == move_base_core::State::SUCCEEDED) + if (nav_state == robot::move_base_core::State::SUCCEEDED) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; action_state->resultDescription = "Done"; @@ -851,11 +850,11 @@ namespace amr_control void VDA5050ClientAPI::pickUp(std::shared_ptr action, std::shared_ptr action_state) { - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { ros::Rate r(5); - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING) { r.sleep(); ros::spinOnce(); @@ -978,18 +977,18 @@ namespace amr_control void VDA5050ClientAPI::dockToStaton(std::shared_ptr action, std::shared_ptr action_state) { - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING; ros::Rate r(5); - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - while (ros::ok() && nav_state == move_base_core::State::CONTROLLING) + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING; r.sleep(); ros::spinOnce(); } - if (nav_state == move_base_core::State::SUCCEEDED) + if (nav_state == robot::move_base_core::State::SUCCEEDED) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED; action_state->resultDescription = "Done"; @@ -1003,23 +1002,23 @@ namespace amr_control void VDA5050ClientAPI::unLoad(std::shared_ptr action, std::shared_ptr action_state) { ROS_INFO("UnLoad running"); - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { ros::Rate r(5); - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; while (ros::ok() && !*enable_action_ && !*this->cancel_action_) { - if (nav_state == move_base_core::State::ABORTED || - nav_state == move_base_core::State::REJECTED) + if (nav_state == robot::move_base_core::State::ABORTED || + nav_state == robot::move_base_core::State::REJECTED) { if (action_state != nullptr) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; - action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; + action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str; } return; } - if (nav_state == move_base_core::State::SUCCEEDED) + if (nav_state == robot::move_base_core::State::SUCCEEDED) break; r.sleep(); } @@ -1081,23 +1080,23 @@ namespace amr_control void VDA5050ClientAPI::load(std::shared_ptr action, std::shared_ptr action_state) { ROS_INFO("Load running"); - if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_) + if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { ros::Rate r(5); - auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; + auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; while (ros::ok() && !*enable_action_ && !*this->cancel_action_) { - if (nav_state == move_base_core::State::ABORTED || - nav_state == move_base_core::State::REJECTED) + if (nav_state == robot::move_base_core::State::ABORTED || + nav_state == robot::move_base_core::State::REJECTED) { if (action_state != nullptr) { action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED; - action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str; + action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str; } return; } - if (nav_state == move_base_core::State::SUCCEEDED) + if (nav_state == robot::move_base_core::State::SUCCEEDED) break; r.sleep(); } @@ -1185,7 +1184,7 @@ namespace amr_control auto &global_visualization = this->client_ptr_->vda5050_visualization_; if (VDA5050ClientAPI::move_base_ptr_) { - geometry_msgs::Pose2D robot_pose; + robot_geometry_msgs::Pose2D robot_pose; if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) { global_visualization.agvPosition.x = robot_pose.x; @@ -1211,9 +1210,9 @@ namespace amr_control std::lock_guard lock(this->client_ptr_->state_mutex); auto &global_state = this->client_ptr_->vda5050_state_; if (VDA5050ClientAPI::move_base_ptr_ != nullptr && - VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr) + VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr) { - geometry_msgs::Pose2D robot_pose; + robot_geometry_msgs::Pose2D robot_pose; if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose)) { global_state.agvPosition.x = robot_pose.x; @@ -1221,14 +1220,14 @@ namespace amr_control global_state.agvPosition.theta = robot_pose.theta; } - move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state; - if (nav_state == move_base_core::State::CONTROLLING) + robot::move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state; + if (nav_state == robot::move_base_core::State::CONTROLLING) { global_state.driving = true; global_state.paused = false; global_state.newBaseRequest = false; } - else if (nav_state == move_base_core::State::PAUSED) + else if (nav_state == robot::move_base_core::State::PAUSED) { global_state.driving = false; global_state.paused = true; @@ -1254,7 +1253,7 @@ namespace amr_control *this->pause_action_ = false; } - std::shared_ptr VDA5050ClientAPI::move_base_ptr_ = nullptr; + std::shared_ptr VDA5050ClientAPI::move_base_ptr_ = nullptr; std::shared_ptr VDA5050ClientAPI::loc_base_ptr_ = nullptr; std::shared_ptr VDA5050ClientAPI::monitor_ptr_ = nullptr; } \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/CMakeLists.txt b/Controllers/Packages/amr_startup/CMakeLists.txt new file mode 100644 index 0000000..b2c0f81 --- /dev/null +++ b/Controllers/Packages/amr_startup/CMakeLists.txt @@ -0,0 +1,205 @@ +cmake_minimum_required(VERSION 3.0.2) +project(amr_startup) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES amr_startup +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/amr_startup.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/amr_startup_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + config + launch + rviz + sdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_startup.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml new file mode 100644 index 0000000..853cace --- /dev/null +++ b/Controllers/Packages/amr_startup/config/base_local_planner_params.yaml @@ -0,0 +1,48 @@ +#For full documentation of the parameters in this file, and a list of all the +#parameters available for TrajectoryPlannerROS, please see +#http://www.ros.org/wiki/base_local_planner +TrajectoryPlannerROS: + #Set the acceleration limits of the robot + acc_lim_th: 3.2 + acc_lim_x: 0.5 + acc_lim_y: 0.5 + + #Set the velocity limits of the robot + max_vel_x: 0.4 + min_vel_x: 0.1 + max_rotational_vel: 1.0 + min_in_place_rotational_vel: 0.4 + + #The velocity the robot will command when trying to escape from a stuck situation + escape_vel: -0.2 + + #For this example, we'll use a holonomic robot + holonomic_robot: false + + #Since we're using a holonomic robot, we'll set the set of y velocities it will sample + y_vels: [-0.3, -0.1, 0.1, 0.3] + + #Set the tolerance on achieving a goal + xy_goal_tolerance: 0.1 + yaw_goal_tolerance: 0.05 + + #We'll configure how long and with what granularity we'll forward simulate trajectories + sim_time: 3.0 + sim_granularity: 0.025 + vx_samples: 5 + vtheta_samples: 20 + + #Parameters for scoring trajectories + goal_distance_bias: 0.4 + path_distance_bias: 0.7 + occdist_scale: 0.3 + heading_lookahead: 0.325 + + #We'll use the Dynamic Window Approach to control instead of Trajectory Rollout for this example + dwa: false + + #How far the robot must travel before oscillation flags are reset + oscillation_reset_dist: 0.01 + + #Eat up the plan as the robot moves along it + prune_plan: false \ 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 new file mode 100755 index 0000000..af52a10 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_common_params.yaml @@ -0,0 +1,54 @@ +robot_base_frame: base_footprint +transform_tolerance: 1.0 +obstacle_range: 3.0 +#mark_threshold: 1 +publish_voxel_map: true +footprint_padding: 0.0 + +navigation_map: + map_topic: /map + map_pkg: managerments + map_file: maze + +virtual_walls_map: + map_topic: /virtual_walls/map + namespace: /virtual_walls + map_pkg: managerments + map_file: maze + use_maximum: true + lethal_cost_threshold: 100 + +obstacles: + observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing + f_scan_marking: + topic: /f_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + f_scan_clearing: + topic: /f_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_marking: + topic: /b_scan + data_type: LaserScan + clearing: false + marking: true + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 + b_scan_clearing: + topic: /b_scan + data_type: LaserScan + clearing: true + marking: false + inf_is_valid: false + min_obstacle_height: 0.0 + max_obstacle_height: 0.25 diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params.yaml new file mode 100755 index 0000000..ae6d769 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_global_params.yaml @@ -0,0 +1,13 @@ +global_costmap: + global_frame: map + update_frequency: 1.0 + publish_frequency: 1.0 + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.2 + rolling_window: false + z_voxels: 10 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.6 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + 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 new file mode 100755 index 0000000..06d0037 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,10 @@ +global_costmap: + frame_id: map + plugins: + - {name: navigation_map, type: "costmap_2d::StaticLayer" } + - {name: virtual_walls_map, type: "costmap_2d::StaticLayer" } + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: false + footprint_clearing_enabled: false \ 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 new file mode 100755 index 0000000..49d126f --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_local_params.yaml @@ -0,0 +1,16 @@ +local_costmap: + global_frame: odom + update_frequency: 5.0 + publish_frequency: 5.0 + rolling_window: true + raytrace_range: 2.0 + resolution: 0.05 + z_resolution: 0.15 + z_voxels: 8 + inflation: + cost_scaling_factor: 10.0 # Exponential rate at which the obstacle cost drops off (default: 10). Must be chosen so that the cost value is > 0 at robot's circumscribed radius. + inflation_radius: 0.3 # Max. distance from an obstacle at which costs are incurred for planning paths. Must be > robot's circumscribed radius. + width: 8.0 + height: 8.0 + origin_x: 0.0 + origin_y: 0.0 \ 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 new file mode 100755 index 0000000..6ef480b --- /dev/null +++ b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -0,0 +1,8 @@ +local_costmap: + frame_id: odom + plugins: + - {name: obstacles, type: "costmap_2d::VoxelLayer" } + - {name: inflation, type: "costmap_2d::InflationLayer" } + obstacles: + enabled: true + footprint_clearing_enabled: true diff --git a/Controllers/Packages/amr_startup/config/custom_global_params.yaml b/Controllers/Packages/amr_startup/config/custom_global_params.yaml new file mode 100644 index 0000000..6536a6e --- /dev/null +++ b/Controllers/Packages/amr_startup/config/custom_global_params.yaml @@ -0,0 +1,17 @@ +base_global_planner: CustomPlanner +CustomPlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: false + nominalvel_mpersecs: 0.8 + timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s + allow_unknown: true + directory_to_save_paths: "/init/paths" + pathway_filename: "pathway.txt" + current_pose_topic_name: "/amcl_pose" + map_frame_id: "map" + base_frame_id: "base_link" + diff --git a/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml new file mode 100755 index 0000000..b8d766a --- /dev/null +++ b/Controllers/Packages/amr_startup/config/dwa_local_planner_params.yaml @@ -0,0 +1,55 @@ +base_local_planner: dwa_local_planner/DWAPlannerROS +DWAPlannerROS: + # Robot configuration + max_vel_x: 0.8 + min_vel_x: -0.2 + + max_vel_y: 0.0 # diff drive robot + min_vel_y: 0.0 # diff drive robot + + max_trans_vel: 0.8 # choose slightly less than the base's capability + min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity + trans_stopped_vel: 0.03 + + # Warning! + # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities + # are non-negligible and small in place rotational velocities will be created. + + max_rot_vel: 1.0 # choose slightly less than the base's capability + min_rot_vel: 0.1 # this is the min angular velocity when there is negligible translational velocity + rot_stopped_vel: 0.1 + + acc_lim_x: 1.5 + acc_lim_y: 0.0 # diff drive robot + acc_limit_trans: 1.5 + acc_lim_theta: 2.0 + + # Goal tolerance + yaw_goal_tolerance: 0.03 # yaw_goal_tolerance > (sim_time * min_rot_vel) / 2 (from Navigation Tuning Guide) + xy_goal_tolerance: 0.08 # xy_goal_tolerance > (sim_time * min_vel_x) / 2 + latch_xy_goal_tolerance: true + + # Forward simulation + sim_time: 1.2 + vx_samples: 15 + vy_samples: 1 # diff drive robot, there is only one sample + vtheta_samples: 20 + + # Trajectory scoring + path_distance_bias: 64.0 # default: 32.0 mir: 32.0 - weighting for how much it should stick to the global path plan + goal_distance_bias: 12.0 # default: 24.0 mir: 48.0 - weighting for how much it should attempt to reach its goal + occdist_scale: 0.5 # default: 0.01 mir: 0.01 - weighting for how much the controller should avoid obstacles + forward_point_distance: 0.325 # default: 0.325 mir: 0.325 - how far along to place an additional scoring point + stop_time_buffer: 0.2 # default: 0.2 mir: 0.2 - amount of time a robot must stop before colliding for a valid traj. + scaling_speed: 0.25 # default: 0.25 mir: 0.25 - absolute velocity at which to start scaling the robot's footprint + max_scaling_factor: 0.2 # default: 0.2 mir: 0.2 - how much to scale the robot's footprint when at speed. + prune_plan: true + + # Oscillation prevention + oscillation_reset_dist: 0.05 # 0.05 - how far to travel before resetting oscillation flags, in m + oscillation_reset_angle: 0.2 # 0.2 - the angle the robot must turn before resetting Oscillation flags, in rad + + # Debugging + publish_traj_pc : true + publish_cost_grid_pc: true + global_frame_id: /odom # or /odom diff --git a/Controllers/Packages/amr_startup/config/ekf.yaml b/Controllers/Packages/amr_startup/config/ekf.yaml new file mode 100755 index 0000000..7344ee3 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/ekf.yaml @@ -0,0 +1,217 @@ +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. +frequency: 50 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. +sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. +two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. +transform_time_offset: 0.0 + +# Use this parameter to specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. +transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. +print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. +debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. +debug_out_file: /path/to/debug/file.txt + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. +publish_tf: true + +# Whether to publish the acceleration state. Defaults to false if unspecified. +publish_acceleration: false + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. +map_frame: map # Defaults to "map" if unspecified +odom_frame: $(arg tf_prefix)odom # Defaults to "odom" if unspecified +base_link_frame: $(arg tf_prefix)base_footprint # Defaults to "base_link" if unspecified +world_frame: $(arg tf_prefix)odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. +odom0: odom + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. +# see http://docs.ros.org/melodic/api/robot_localization/html/configuring_robot_localization.html +odom0_config: [false, false, false, # x y z + false, false, false, # roll pitch yaw + true, true, false, # vx vy vz + false, false, true, # vroll vpitch vyaw + false, false, false] # ax ay az + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. +odom0_queue_size: 10 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. +odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. +odom0_differential: false + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. +odom0_relative: false + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::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 new file mode 100644 index 0000000..677cb96 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/localization.yaml @@ -0,0 +1,36 @@ +Amcl: + use_map_topic: true + odom_model_type: "diff-corrected" + gui_publish_rate: 5.0 + save_pose_rate: 0.5 + laser_max_beams: 300 + laser_min_range: -1.0 + laser_max_range: -1.0 + min_particles: 1000 + max_particles: 3000 + kld_err: 0.05 + kld_z: 0.99 + odom_alpha1: 0.02 + odom_alpha2: 0.01 + odom_alpha3: 0.01 + odom_alpha4: 0.02 + laser_z_hit: 0.5 + laser_z_short: 0.05 + laser_z_max: 0.05 + laser_z_rand: 0.5 + laser_sigma_hit: 0.2 + laser_lambda_short: 0.1 + laser_model_type: "likelihood_field" + laser_likelihood_max_dist: 1.0 + update_min_d: 0.05 + update_min_a: 0.05 + odom_frame_id: odom + base_frame_id: base_footprint + global_frame_id: map + resample_interval: 1 + transform_tolerance: 0.2 + recovery_alpha_slow: 0.001 + recovery_alpha_fast: 0.001 + initial_pose_x: 0.0 + initial_pose_y: 0.0 + initial_pose_a: 0.0 diff --git a/Controllers/Packages/amr_startup/config/maker_sources.yaml b/Controllers/Packages/amr_startup/config/maker_sources.yaml new file mode 100644 index 0000000..2bee0f9 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/maker_sources.yaml @@ -0,0 +1,120 @@ +maker_sources: trolley charger dock_station undock_station +trolley: + plugins: + - {name: 4legs, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + 4legs: + maker_goal_frame: trolley_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2.0 + timeout: 60.0 + vel_x: 0.1 + vel_theta: 0.3 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + qrcode: + maker_goal_frame: qr_trolley + delay: 2.0 + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.2 + allow_rotate: true + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +charger: + plugins: + - {name: charger, docking_planner: "DockPlanner", docking_nav: ""} + + charger: + maker_goal_frame: charger_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2 + timeout: 60 + vel_x: 0.1 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +dock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 2 + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + +dock_station_2: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + + station: + maker_goal_frame: dock_station_goal_2 + footprint: [[1.15,-0.55],[1.15,0.55],[-1.15,0.55],[-1.15,-0.55]] + delay: 2 + timeout: 60 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + +undock_station: + plugins: + - {name: station, docking_planner: "DockPlanner", docking_nav: ""} + - {name: qrcode, docking_planner: "TwoPointsPlanner", docking_nav: "" } + + station: + maker_goal_frame: undock_station_goal + footprint: [[0.583,-0.48],[0.583,0.48],[-0.583,0.48],[-0.583,-0.48]] + delay: 2.0 + timeout: 60.0 + vel_x: 0.15 + vel_theta: 0.3 + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 + + qrcode: + maker_goal_frame: qr_station + delay: 2.0 + timeout: 60.0 + vel_x: 0.05 + vel_theta: 0.2 + allow_rotate: true + yaw_goal_tolerance: 0.01 + xy_goal_tolerance: 0.01 + min_lookahead_dist: 0.4 + max_lookahead_dist: 1.0 + lookahead_time: 1.5 + angle_threshold: 0.36 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/config/mapping.yaml b/Controllers/Packages/amr_startup/config/mapping.yaml new file mode 100644 index 0000000..e58fce3 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mapping.yaml @@ -0,0 +1,63 @@ +SlamToolBox: + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + mode: mapping #localization + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 10.0 + resolution: 0.05 + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 14400. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true diff --git a/Controllers/Packages/amr_startup/config/move_base_common_params.yaml b/Controllers/Packages/amr_startup/config/move_base_common_params.yaml new file mode 100755 index 0000000..6d275d8 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/move_base_common_params.yaml @@ -0,0 +1,24 @@ + +### replanning +controller_frequency: 20.0 # run controller at 15.0 Hz +controller_patience: 0.0 # if the controller failed, clear obstacles and retry; after 15.0 s, abort and replan +planner_frequency: 0.0 # don't continually replan (only when controller failed) +planner_patience: 5.0 # if the first planning attempt failed, abort planning retries after 5.0 s... +max_planning_retries: 5 # ... or after 10 attempts (whichever happens first) +oscillation_timeout: -1.0 # abort controller and trigger recovery behaviors after 30.0 s +oscillation_distance: 1.5 +### recovery behaviors +recovery_behavior_enabled: false +recovery_behaviors: [ + {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + ] + +conservative_reset: + reset_distance: 3.0 # clear obstacles farther away than 3.0 m + invert_area_to_clear: true + +aggressive_reset: + reset_distance: 3.0 + + diff --git a/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml b/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml new file mode 100755 index 0000000..aa6a592 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mpc_local_planner_params.yaml @@ -0,0 +1,106 @@ +base_local_planner: mpc_local_planner/MpcLocalPlannerROS +MpcLocalPlannerROS: + + odom_topic: odom + + ## Robot settings + robot: + type: "unicycle" + unicycle: + max_vel_x: 0.5 + max_vel_x_backwards: 0.3 + max_vel_theta: 0.3 + acc_lim_x: 0.4 # deactive bounds with zero + dec_lim_x: 0.4 # deactive bounds with zero + acc_lim_theta: 0.6 # deactivate bounds with zero + + ## Footprint model for collision avoidance + footprint_model: + type: "point" + is_footprint_dynamic: False + + ## Collision avoidance + collision_avoidance: + min_obstacle_dist: 0.5 # Note, this parameter must be chosen w.r.t. the footprint_model + enable_dynamic_obstacles: False + force_inclusion_dist: 0.5 + cutoff_dist: 2.0 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + collision_check_no_poses: 5 + + ## Planning grid + grid: + type: "fd_grid" + grid_size_ref: 20 # Set horizon length here (T = (grid_size_ref-1) * dt_ref); Note, also check max_global_plan_lookahead_dist + dt_ref: 0.3 # and here the corresponding temporal resolution + xf_fixed: [False, False, False] # Unfix final state -> we use terminal cost below + warm_start: True + collocation_method: "forward_differences" + cost_integration_method: "left_sum" + variable_grid: + enable: False # We want a fixed grid + min_dt: 0.0; + max_dt: 10.0; + grid_adaptation: + enable: True + dt_hyst_ratio: 0.1 + min_grid_size: 2 + max_grid_size: 50 + + ## Planning options + planning: + objective: + type: "quadratic_form" # minimum_time requires grid/variable_grid/enable=True and grid/xf_fixed set properly + quadratic_form: + state_weights: [2.0, 2.0, 0.25] + control_weights: [0.1, 0.05] + integral_form: False + terminal_cost: + type: "quadratic" # can be "none" + quadratic: + final_state_weights: [10.0, 10.0, 0.5] + terminal_constraint: + type: "none" # can be "none" + l2_ball: + weight_matrix: [1.0, 1.0, 1.0] + radius: 5 + + ## Controller options + controller: + outer_ocp_iterations: 1 + xy_goal_tolerance: 0.05 + yaw_goal_tolerance: 0.04 + global_plan_overwrite_orientation: False + global_plan_prune_distance: 1.5 + allow_init_with_backward_motion: True + max_global_plan_lookahead_dist: 1.0 # Check horizon length + force_reinit_new_goal_dist: 1.0 + force_reinit_new_goal_angular: 1.57 + force_reinit_num_steps: 0 + prefer_x_feedback: False + publish_ocp_results: True + + ## Solver settings + solver: + type: "ipopt" + ipopt: + iterations: 100 + max_cpu_time: -1.0 + ipopt_numeric_options: + tol: 1e-3 + ipopt_string_options: + linear_solver: "mumps" + hessian_approximation: "exact" # exact or limited-memory + lsq_lm: + iterations: 10 + weight_init_eq: 2 + weight_init_ineq: 2 + weight_init_bounds: 2 + weight_adapt_factor_eq: 1.5 + weight_adapt_factor_ineq: 1.5 + weight_adapt_factor_bounds: 1.5 + weight_adapt_max_eq: 500 + weight_adapt_max_ineq: 500 + weight_adapt_max_bounds: 500 + 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 new file mode 100755 index 0000000..2810593 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.m @@ -0,0 +1,280 @@ +% /* +% * Copyright (c) 2008, Maxim Likhachev +% * All rights reserved. +% * +% * Redistribution and use in source and binary forms, with or without +% * modification, are permitted provided that the following conditions are met: +% * +% * * Redistributions of source code must retain the above copyright +% * notice, this list of conditions and the following disclaimer. +% * * Redistributions in binary form must reproduce the above copyright +% * notice, this list of conditions and the following disclaimer in the +% * documentation and/or other materials provided with the distribution. +% * * Neither the name of the Carnegie Mellon University nor the names of its +% * contributors may be used to endorse or promote products derived from +% * this software without specific prior written permission. +% * +% * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +% * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +% * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +% * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +% * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +% * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +% * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +% * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +% * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +% * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +% * POSSIBILITY OF SUCH DAMAGE. +% */ + +function[] = genmprim_unicycle_highcost_5cm(outfilename) + +% +%generates motion primitives and saves them into file +% +%written by Maxim Likhachev +%--------------------------------------------------- +% + +%defines + +UNICYCLE_MPRIM_16DEGS = 1; + + +if UNICYCLE_MPRIM_16DEGS == 1 + resolution = 0.05; + numberofangles = 16; %preferably a power of 2, definitely multiple of 8 + numberofprimsperangle = 7; + + %multipliers (multiplier is used as costmult*cost) + forwardcostmult = 1; + backwardcostmult = 40; + forwardandturncostmult = 2; + sidestepcostmult = 10; + turninplacecostmult = 20; + + %note, what is shown x,y,theta changes (not absolute numbers) + + %0 degreees + basemprimendpts0_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts0_c(1,:) = [1 0 0 forwardcostmult]; + basemprimendpts0_c(2,:) = [8 0 0 forwardcostmult]; + basemprimendpts0_c(3,:) = [-1 0 0 backwardcostmult]; + %1/16 theta change + basemprimendpts0_c(4,:) = [8 1 1 forwardandturncostmult]; + basemprimendpts0_c(5,:) = [8 -1 -1 forwardandturncostmult]; + %turn in place + basemprimendpts0_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts0_c(7,:) = [0 0 -1 turninplacecostmult]; + + %45 degrees + basemprimendpts45_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts45_c(1,:) = [1 1 0 forwardcostmult]; + basemprimendpts45_c(2,:) = [6 6 0 forwardcostmult]; + basemprimendpts45_c(3,:) = [-1 -1 0 backwardcostmult]; + %1/16 theta change + basemprimendpts45_c(4,:) = [5 7 1 forwardandturncostmult]; + basemprimendpts45_c(5,:) = [7 5 -1 forwardandturncostmult]; + %turn in place + basemprimendpts45_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts45_c(7,:) = [0 0 -1 turninplacecostmult]; + + %22.5 degrees + basemprimendpts22p5_c = zeros(numberofprimsperangle, 4); %x,y,theta,costmult (multiplier is used as costmult*cost) + %x aligned with the heading of the robot, angles are positive + %counterclockwise + %0 theta change + basemprimendpts22p5_c(1,:) = [2 1 0 forwardcostmult]; + basemprimendpts22p5_c(2,:) = [6 3 0 forwardcostmult]; + basemprimendpts22p5_c(3,:) = [-2 -1 0 backwardcostmult]; + %1/16 theta change + basemprimendpts22p5_c(4,:) = [5 4 1 forwardandturncostmult]; + basemprimendpts22p5_c(5,:) = [7 2 -1 forwardandturncostmult]; + %turn in place + basemprimendpts22p5_c(6,:) = [0 0 1 turninplacecostmult]; + basemprimendpts22p5_c(7,:) = [0 0 -1 turninplacecostmult]; + +else + fprintf(1, 'ERROR: undefined mprims type\n'); + return; +end; + + +fout = fopen(outfilename, 'w'); + + +%write the header +fprintf(fout, 'resolution_m: %f\n', resolution); +fprintf(fout, 'numberofangles: %d\n', numberofangles); +fprintf(fout, 'totalnumberofprimitives: %d\n', numberofprimsperangle*numberofangles); + +%iterate over angles +for angleind = 1:numberofangles + + figure(1); + hold off; + + text(0, 0, int2str(angleind)); + + %iterate over primitives + for primind = 1:numberofprimsperangle + fprintf(fout, 'primID: %d\n', primind-1); + fprintf(fout, 'startangle_c: %d\n', angleind-1); + + %current angle + currentangle = (angleind-1)*2*pi/numberofangles; + currentangle_36000int = round((angleind-1)*36000/numberofangles); + + %compute which template to use + if (rem(currentangle_36000int, 9000) == 0) + basemprimendpts_c = basemprimendpts0_c(primind,:); + angle = currentangle; + elseif (rem(currentangle_36000int, 4500) == 0) + basemprimendpts_c = basemprimendpts45_c(primind,:); + angle = currentangle - 45*pi/180; + elseif (rem(currentangle_36000int-7875, 9000) == 0) + basemprimendpts_c = basemprimendpts33p75_c(primind,:); + basemprimendpts_c(1) = basemprimendpts33p75_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts33p75_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts33p75_c(primind, 3); %reverse the angle as well + angle = currentangle - 78.75*pi/180; + fprintf(1, '78p75\n'); + elseif (rem(currentangle_36000int-6750, 9000) == 0) + basemprimendpts_c = basemprimendpts22p5_c(primind,:); + basemprimendpts_c(1) = basemprimendpts22p5_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts22p5_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts22p5_c(primind, 3); %reverse the angle as well + %fprintf(1, '%d %d %d onto %d %d %d\n', basemprimendpts22p5_c(1), basemprimendpts22p5_c(2), basemprimendpts22p5_c(3), ... + % basemprimendpts_c(1), basemprimendpts_c(2), basemprimendpts_c(3)); + angle = currentangle - 67.5*pi/180; + fprintf(1, '67p5\n'); + elseif (rem(currentangle_36000int-5625, 9000) == 0) + basemprimendpts_c = basemprimendpts11p25_c(primind,:); + basemprimendpts_c(1) = basemprimendpts11p25_c(primind, 2); %reverse x and y + basemprimendpts_c(2) = basemprimendpts11p25_c(primind, 1); + basemprimendpts_c(3) = -basemprimendpts11p25_c(primind, 3); %reverse the angle as well + angle = currentangle - 56.25*pi/180; + fprintf(1, '56p25\n'); + elseif (rem(currentangle_36000int-3375, 9000) == 0) + basemprimendpts_c = basemprimendpts33p75_c(primind,:); + angle = currentangle - 33.75*pi/180; + fprintf(1, '33p75\n'); + elseif (rem(currentangle_36000int-2250, 9000) == 0) + basemprimendpts_c = basemprimendpts22p5_c(primind,:); + angle = currentangle - 22.5*pi/180; + fprintf(1, '22p5\n'); + elseif (rem(currentangle_36000int-1125, 9000) == 0) + basemprimendpts_c = basemprimendpts11p25_c(primind,:); + angle = currentangle - 11.25*pi/180; + fprintf(1, '11p25\n'); + else + fprintf(1, 'ERROR: invalid angular resolution. angle = %d\n', currentangle_36000int); + return; + end; + + %now figure out what action will be + baseendpose_c = basemprimendpts_c(1:3); + additionalactioncostmult = basemprimendpts_c(4); + endx_c = round(baseendpose_c(1)*cos(angle) - baseendpose_c(2)*sin(angle)); + endy_c = round(baseendpose_c(1)*sin(angle) + baseendpose_c(2)*cos(angle)); + endtheta_c = rem(angleind - 1 + baseendpose_c(3), numberofangles); + endpose_c = [endx_c endy_c endtheta_c]; + + fprintf(1, 'rotation angle=%f\n', angle*180/pi); + + if baseendpose_c(2) == 0 & baseendpose_c(3) == 0 + %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + end; + + %generate intermediate poses (remember they are w.r.t 0,0 (and not + %centers of the cells) + numofsamples = 10; + intermcells_m = zeros(numofsamples,3); + if UNICYCLE_MPRIM_16DEGS == 1 + startpt = [0 0 currentangle]; + endpt = [endpose_c(1)*resolution endpose_c(2)*resolution ... + rem(angleind - 1 + baseendpose_c(3), numberofangles)*2*pi/numberofangles]; + intermcells_m = zeros(numofsamples,3); + if ((endx_c == 0 & endy_c == 0) | baseendpose_c(3) == 0) %turn in place or move forward + for iind = 1:numofsamples + intermcells_m(iind,:) = [startpt(1) + (endpt(1) - startpt(1))*(iind-1)/(numofsamples-1) ... + startpt(2) + (endpt(2) - startpt(2))*(iind-1)/(numofsamples-1) ... + 0]; + rotation_angle = (baseendpose_c(3) ) * (2*pi/numberofangles); + intermcells_m(iind,3) = rem(startpt(3) + (rotation_angle)*(iind-1)/(numofsamples-1), 2*pi); + + end; + else %unicycle-based move forward or backward + R = [cos(startpt(3)) sin(endpt(3)) - sin(startpt(3)); + sin(startpt(3)) -(cos(endpt(3)) - cos(startpt(3)))]; + S = pinv(R)*[endpt(1) - startpt(1); endpt(2) - startpt(2)]; + l = S(1); + tvoverrv = S(2); + rv = (baseendpose_c(3)*2*pi/numberofangles + l/tvoverrv); + tv = tvoverrv*rv; + + if l < 0 + fprintf(1, 'WARNING: l = %d < 0 -> bad action start/end points\n', l); + l = 0; + end; + %compute rv + %rv = baseendpose_c(3)*2*pi/numberofangles; + %compute tv + %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) + %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) + %tv = (tvx + tvy)/2.0; + %generate samples + for iind = 1:numofsamples + dt = (iind-1)/(numofsamples-1); + + %dtheta = rv*dt + startpt(3); + %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... + % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... + % dtheta]; + + if(dt*tv < l) + intermcells_m(iind,:) = [startpt(1) + dt*tv*cos(startpt(3)) ... + startpt(2) + dt*tv*sin(startpt(3)) ... + startpt(3)]; + else + dtheta = rv*(dt - l/tv) + startpt(3); + intermcells_m(iind,:) = [startpt(1) + l*cos(startpt(3)) + tvoverrv*(sin(dtheta) - sin(startpt(3))) ... + startpt(2) + l*sin(startpt(3)) - tvoverrv*(cos(dtheta) - cos(startpt(3))) ... + dtheta]; + end; + end; + %correct + errorxy = [endpt(1) - intermcells_m(numofsamples,1) ... + endpt(2) - intermcells_m(numofsamples,2)]; + fprintf(1, 'l=%f errx=%f erry=%f\n', l, errorxy(1), errorxy(2)); + interpfactor = [0:1/(numofsamples-1):1]; + intermcells_m(:,1) = intermcells_m(:,1) + errorxy(1)*interpfactor'; + intermcells_m(:,2) = intermcells_m(:,2) + errorxy(2)*interpfactor'; + end; + end; + + %write out + fprintf(fout, 'endpose_c: %d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + fprintf(fout, 'additionalactioncostmult: %d\n', additionalactioncostmult); + fprintf(fout, 'intermediateposes: %d\n', size(intermcells_m,1)); + for interind = 1:size(intermcells_m, 1) + fprintf(fout, '%.4f %.4f %.4f\n', intermcells_m(interind,1), intermcells_m(interind,2), intermcells_m(interind,3)); + end; + + plot(intermcells_m(:,1), intermcells_m(:,2)); + axis([-0.3 0.3 -0.3 0.3]); + text(intermcells_m(numofsamples,1), intermcells_m(numofsamples,2), int2str(endpose_c(3))); + hold on; + + end; + grid; + pause; +end; + +fclose('all'); 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 new file mode 100755 index 0000000..c8801c0 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/genmprim_unicycle_highcost_5cm.py @@ -0,0 +1,416 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2016, David Conner (Christopher Newport University) +# Based on genmprim_unicycle.m +# Copyright (c) 2008, Maxim Likhachev +# All rights reserved. +# converted by libermate utility (https://github.com/awesomebytes/libermate) +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Carnegie Mellon University nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import numpy as np +import rospkg + +# if available import pylab (from matlibplot) +matplotlib_found = False +try: + import matplotlib.pylab as plt + + matplotlib_found = True +except ImportError: + pass + + +def matrix_size(mat, elem=None): + if not elem: + return mat.shape + else: + return mat.shape[int(elem) - 1] + + +def genmprim_unicycle(outfilename, visualize=False, separate_plots=False): + visualize = matplotlib_found and visualize # Plot the primitives + + # Local Variables: basemprimendpts22p5_c, endtheta_c, endx_c, + # baseendpose_c, additionalactioncostmult, fout, numofsamples, + # basemprimendpts45_c, primind, basemprimendpts0_c, rv, angle, outfilename, + # numberofangles, startpt, UNICYCLE_MPRIM_16DEGS, sidestepcostmult, + # rotation_angle, basemprimendpts_c, forwardandturncostmult, + # forwardcostmult, turninplacecostmult, endpose_c, backwardcostmult, + # interpfactor, S, R, tvoverrv, dtheta, intermcells_m, tv, dt, + # currentangle, numberofprimsperangle, resolution, currentangle_36000int, + # l, iind, errorxy, interind, endy_c, angleind, endpt + # Function calls: plot, cos, pi, grid, figure, genmprim_unicycle, text, + # int2str, pause, axis, sin, pinv, fprintf, fclose, rem, zeros, fopen, + # round, size + # % + # %generates motion primitives and saves them into file + # % + # %written by Maxim Likhachev + # %--------------------------------------------------- + # % + # %defines + UNICYCLE_MPRIM_16DEGS = 1.0 + if UNICYCLE_MPRIM_16DEGS == 1.0: + resolution = 0.05 + numberofangles = 16 + # %preferably a power of 2, definitely multiple of 8 + numberofprimsperangle = 7 + # %multipliers (multiplier is used as costmult*cost) + forwardcostmult = 1.0 + backwardcostmult = 40.0 + forwardandturncostmult = 2.0 + # sidestepcostmult = 10.0 + turninplacecostmult = 20.0 + # %note, what is shown x,y,theta changes (not absolute numbers) + # %0 degreees + basemprimendpts0_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts0_c[0, :] = np.array(np.hstack((1.0, 0.0, 0.0, forwardcostmult))) + basemprimendpts0_c[1, :] = np.array(np.hstack((8.0, 0.0, 0.0, forwardcostmult))) + basemprimendpts0_c[2, :] = np.array(np.hstack((-1.0, 0.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts0_c[3, :] = np.array(np.hstack((8.0, 1.0, 1.0, forwardandturncostmult))) + basemprimendpts0_c[4, :] = np.array(np.hstack((8.0, -1.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts0_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts0_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + # %45 degrees + basemprimendpts45_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult (multiplier is used as costmult*cost) + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts45_c[0, :] = np.array(np.hstack((1.0, 1.0, 0.0, forwardcostmult))) + basemprimendpts45_c[1, :] = np.array(np.hstack((6.0, 6.0, 0.0, forwardcostmult))) + basemprimendpts45_c[2, :] = np.array(np.hstack((-1.0, -1.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts45_c[3, :] = np.array(np.hstack((5.0, 7.0, 1.0, forwardandturncostmult))) + basemprimendpts45_c[4, :] = np.array(np.hstack((7.0, 5.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts45_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts45_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + # %22.5 degrees + basemprimendpts22p5_c = np.zeros((numberofprimsperangle, 4)) + # %x,y,theta,costmult (multiplier is used as costmult*cost) + # %x aligned with the heading of the robot, angles are positive + # %counterclockwise + # %0 theta change + basemprimendpts22p5_c[0, :] = np.array(np.hstack((2.0, 1.0, 0.0, forwardcostmult))) + basemprimendpts22p5_c[1, :] = np.array(np.hstack((6.0, 3.0, 0.0, forwardcostmult))) + basemprimendpts22p5_c[2, :] = np.array(np.hstack((-2.0, -1.0, 0.0, backwardcostmult))) + # %1/16 theta change + basemprimendpts22p5_c[3, :] = np.array(np.hstack((5.0, 4.0, 1.0, forwardandturncostmult))) + basemprimendpts22p5_c[4, :] = np.array(np.hstack((7.0, 2.0, -1.0, forwardandturncostmult))) + # %turn in place + basemprimendpts22p5_c[5, :] = np.array(np.hstack((0.0, 0.0, 1.0, turninplacecostmult))) + basemprimendpts22p5_c[6, :] = np.array(np.hstack((0.0, 0.0, -1.0, turninplacecostmult))) + else: + print('ERROR: undefined mprims type\n') + return [] + + fout = open(outfilename, 'w') + # %write the header + fout.write('resolution_m: %f\n' % (resolution)) + fout.write('numberofangles: %d\n' % (numberofangles)) + fout.write('totalnumberofprimitives: %d\n' % (numberofprimsperangle * numberofangles)) + # %iterate over angles + for angleind in np.arange(1.0, (numberofangles) + 1): + currentangle = ((angleind - 1) * 2.0 * np.pi) / numberofangles + currentangle_36000int = np.round((angleind - 1) * 36000.0 / numberofangles) + if visualize: + if separate_plots: + fig = plt.figure(angleind) + plt.title('angle {:2.0f} (= {:3.1f} degrees)'.format(angleind - 1, currentangle_36000int / 100.0)) + else: + fig = plt.figure(1) + + plt.axis('equal') + plt.axis([-10 * resolution, 10 * resolution, -10 * resolution, 10 * resolution]) + ax = fig.add_subplot(1, 1, 1) + major_ticks = np.arange(-8 * resolution, 9 * resolution, 4 * resolution) + minor_ticks = np.arange(-8 * resolution, 9 * resolution, resolution) + ax.set_xticks(major_ticks) + ax.set_xticks(minor_ticks, minor=True) + ax.set_yticks(major_ticks) + ax.set_yticks(minor_ticks, minor=True) + ax.grid(which='minor', alpha=0.5) + ax.grid(which='major', alpha=0.9) + + # %iterate over primitives + for primind in np.arange(1.0, (numberofprimsperangle) + 1): + fout.write('primID: %d\n' % (primind - 1)) + fout.write('startangle_c: %d\n' % (angleind - 1)) + # %current angle + # %compute which template to use + if (currentangle_36000int % 9000) == 0: + basemprimendpts_c = basemprimendpts0_c[int(primind) - 1, :] + angle = currentangle + elif (currentangle_36000int % 4500) == 0: + basemprimendpts_c = basemprimendpts45_c[int(primind) - 1, :] + angle = currentangle - 45.0 * np.pi / 180.0 + + # commented out because basemprimendpts33p75_c is undefined + # elif ((currentangle_36000int - 7875) % 9000) == 0: + # basemprimendpts_c = ( + # 1 * basemprimendpts33p75_c[primind, :] + # ) # 1* to force deep copy to avoid reference update below + # basemprimendpts_c[0] = basemprimendpts33p75_c[primind, 1] + # # %reverse x and y + # basemprimendpts_c[1] = basemprimendpts33p75_c[primind, 0] + # basemprimendpts_c[2] = -basemprimendpts33p75_c[primind, 2] + # # %reverse the angle as well + # angle = currentangle - (78.75 * np.pi) / 180.0 + # print('78p75\n') + + elif ((currentangle_36000int - 6750) % 9000) == 0: + basemprimendpts_c = ( + 1 * basemprimendpts22p5_c[int(primind) - 1, :] + ) # 1* to force deep copy to avoid reference update below + basemprimendpts_c[0] = basemprimendpts22p5_c[int(primind) - 1, 1] + # %reverse x and y + basemprimendpts_c[1] = basemprimendpts22p5_c[int(primind) - 1, 0] + basemprimendpts_c[2] = -basemprimendpts22p5_c[int(primind) - 1, 2] + # %reverse the angle as well + # print( + # '%d : %d %d %d onto %d %d %d\n' + # % ( + # primind - 1, + # basemprimendpts22p5_c[int(primind) - 1, 0], + # basemprimendpts22p5_c[int(primind) - 1, 1], + # basemprimendpts22p5_c[int(primind) - 1, 2], + # basemprimendpts_c[0], + # basemprimendpts_c[1], + # basemprimendpts_c[2], + # ) + # ) + angle = currentangle - (67.5 * np.pi) / 180.0 + print('67p5\n') + + # commented out because basemprimendpts11p25_c is undefined + # elif ((currentangle_36000int - 5625) % 9000) == 0: + # basemprimendpts_c = ( + # 1 * basemprimendpts11p25_c[primind, :] + # ) # 1* to force deep copy to avoid reference update below + # basemprimendpts_c[0] = basemprimendpts11p25_c[primind, 1] + # # %reverse x and y + # basemprimendpts_c[1] = basemprimendpts11p25_c[primind, 0] + # basemprimendpts_c[2] = -basemprimendpts11p25_c[primind, 2] + # # %reverse the angle as well + # angle = currentangle - (56.25 * np.pi) / 180.0 + # print('56p25\n') + + # commented out because basemprimendpts33p75_c is undefined + # elif ((currentangle_36000int - 3375) % 9000) == 0: + # basemprimendpts_c = basemprimendpts33p75_c[int(primind), :] + # angle = currentangle - (33.75 * np.pi) / 180.0 + # print('33p75\n') + + elif ((currentangle_36000int - 2250) % 9000) == 0: + basemprimendpts_c = basemprimendpts22p5_c[int(primind) - 1, :] + angle = currentangle - (22.5 * np.pi) / 180.0 + print('22p5\n') + + # commented out because basemprimendpts11p25_c is undefined + # elif ((currentangle_36000int - 1125) % 9000) == 0: + # basemprimendpts_c = basemprimendpts11p25_c[int(primind), :] + # angle = currentangle - (11.25 * np.pi) / 180.0 + # print('11p25\n') + + else: + print('ERROR: invalid angular resolution. angle = %d\n' % currentangle_36000int) + return [] + + # %now figure out what action will be + baseendpose_c = basemprimendpts_c[0:3] + additionalactioncostmult = basemprimendpts_c[3] + endx_c = np.round((baseendpose_c[0] * np.cos(angle)) - (baseendpose_c[1] * np.sin(angle))) + endy_c = np.round((baseendpose_c[0] * np.sin(angle)) + (baseendpose_c[1] * np.cos(angle))) + endtheta_c = np.fmod(angleind - 1 + baseendpose_c[2], numberofangles) + endpose_c = np.array(np.hstack((endx_c, endy_c, endtheta_c))) + print("endpose_c=", endpose_c) + print(('rotation angle=%f\n' % (angle * 180.0 / np.pi))) + # if np.logical_and(baseendpose_c[1] == 0., baseendpose_c[2] == 0.): + # %fprintf(1, 'endpose=%d %d %d\n', endpose_c(1), endpose_c(2), endpose_c(3)); + + # %generate intermediate poses (remember they are w.r.t 0,0 (and not + # %centers of the cells) + numofsamples = 10 + intermcells_m = np.zeros((numofsamples, 3)) + if UNICYCLE_MPRIM_16DEGS == 1.0: + startpt = np.array(np.hstack((0.0, 0.0, currentangle))) + endpt = np.array( + np.hstack( + ( + (endpose_c[0] * resolution), + (endpose_c[1] * resolution), + ( + ((np.fmod(angleind - 1 + baseendpose_c[2], numberofangles)) * 2.0 * np.pi) + / numberofangles + ), + ) + ) + ) + + print("startpt =", startpt) + print("endpt =", endpt) + intermcells_m = np.zeros((numofsamples, 3)) + if np.logical_or(np.logical_and(endx_c == 0.0, endy_c == 0.0), baseendpose_c[2] == 0.0): + # %turn in place or move forward + for iind in np.arange(1.0, (numofsamples) + 1): + fraction = float(iind - 1) / (numofsamples - 1) + intermcells_m[int(iind) - 1, :] = np.array( + ( + startpt[0] + (endpt[0] - startpt[0]) * fraction, + startpt[1] + (endpt[1] - startpt[1]) * fraction, + 0, + ) + ) + rotation_angle = baseendpose_c[2] * (2.0 * np.pi / numberofangles) + intermcells_m[int(iind) - 1, 2] = np.fmod(startpt[2] + rotation_angle * fraction, (2.0 * np.pi)) + # print " ",iind," of ",numofsamples," fraction=",fraction," rotation=",rotation_angle + + else: + # %unicycle-based move forward or backward (http://sbpl.net/node/53) + R = np.array( + np.vstack( + ( + np.hstack((np.cos(startpt[2]), np.sin(endpt[2]) - np.sin(startpt[2]))), + np.hstack((np.sin(startpt[2]), -np.cos(endpt[2]) + np.cos(startpt[2]))), + ) + ) + ) + + S = np.dot(np.linalg.pinv(R), np.array(np.vstack((endpt[0] - startpt[0], endpt[1] - startpt[1])))) + l = S[0] + tvoverrv = S[1] + rv = (baseendpose_c[2] * 2.0 * np.pi / numberofangles) + l / tvoverrv + tv = tvoverrv * rv + + # print "R=\n",R + # print "Rpi=\n",np.linalg.pinv(R) + # print "S=\n",S + # print "l=",l + # print "tvoverrv=",tvoverrv + # print "rv=",rv + # print "tv=",tv + + if l < 0.0: + print(('WARNING: l = %f < 0 -> bad action start/end points\n' % (l))) + l = 0.0 + + # %compute rv + # %rv = baseendpose_c(3)*2*pi/numberofangles; + # %compute tv + # %tvx = (endpt(1) - startpt(1))*rv/(sin(endpt(3)) - sin(startpt(3))) + # %tvy = -(endpt(2) - startpt(2))*rv/(cos(endpt(3)) - cos(startpt(3))) + # %tv = (tvx + tvy)/2.0; + # %generate samples + for iind in np.arange(1, numofsamples + 1): + dt = (iind - 1) / (numofsamples - 1) + # %dtheta = rv*dt + startpt(3); + # %intermcells_m(iind,:) = [startpt(1) + tv/rv*(sin(dtheta) - sin(startpt(3))) ... + # % startpt(2) - tv/rv*(cos(dtheta) - cos(startpt(3))) ... + # % dtheta]; + if (dt * tv) < l: + intermcells_m[int(iind) - 1, :] = np.array( + np.hstack( + ( + startpt[0] + dt * tv * np.cos(startpt[2]), + startpt[1] + dt * tv * np.sin(startpt[2]), + startpt[2], + ) + ) + ) + else: + dtheta = rv * (dt - l / tv) + startpt[2] + intermcells_m[int(iind) - 1, :] = np.array( + np.hstack( + ( + startpt[0] + + l * np.cos(startpt[2]) + + tvoverrv * (np.sin(dtheta) - np.sin(startpt[2])), + startpt[1] + + l * np.sin(startpt[2]) + - tvoverrv * (np.cos(dtheta) - np.cos(startpt[2])), + dtheta, + ) + ) + ) + + # %correct + errorxy = np.array( + np.hstack( + ( + endpt[0] - intermcells_m[int(numofsamples) - 1, 0], + endpt[1] - intermcells_m[int(numofsamples) - 1, 1], + ) + ) + ) + # print('l=%f errx=%f erry=%f\n'%(l, errorxy[0], errorxy[1])) + interpfactor = np.array( + np.hstack((np.arange(0.0, 1.0 + (1.0 / (numofsamples)), 1.0 / (numofsamples - 1)))) + ) + + # print "intermcells_m=",intermcells_m + # print "interp'=",interpfactor.conj().T + + intermcells_m[:, 0] = intermcells_m[:, 0] + errorxy[0] * interpfactor.conj().T + intermcells_m[:, 1] = intermcells_m[:, 1] + errorxy[1] * interpfactor.conj().T + + # %write out + fout.write('endpose_c: %d %d %d\n' % (endpose_c[0], endpose_c[1], endpose_c[2])) + fout.write('additionalactioncostmult: %d\n' % (additionalactioncostmult)) + fout.write('intermediateposes: %d\n' % (matrix_size(intermcells_m, 1.0))) + for interind in np.arange(1.0, (matrix_size(intermcells_m, 1.0)) + 1): + fout.write( + '%.4f %.4f %.4f\n' + % ( + intermcells_m[int(interind) - 1, 0], + intermcells_m[int(interind) - 1, 1], + intermcells_m[int(interind) - 1, 2], + ) + ) + + if visualize: + plt.plot(intermcells_m[:, 0], intermcells_m[:, 1], linestyle="-", marker="o") + plt.text(endpt[0], endpt[1], '{:2.0f}'.format(endpose_c[2])) + # if (visualize): + # plt.waitforbuttonpress() # uncomment to plot each primitive set one at a time + + fout.close() + if visualize: + # plt.waitforbuttonpress() # hold until buttom pressed + plt.show() # Keep windows open until the program is terminated + return [] + + +if __name__ == "__main__": + rospack = rospkg.RosPack() + outfilename = rospack.get_path('mir_navigation') + '/mprim/unicycle_highcost_5cm.mprim' + genmprim_unicycle(outfilename, visualize=True) diff --git a/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim new file mode 100755 index 0000000..cb56cd0 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm.mprim @@ -0,0 +1,1203 @@ +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 new file mode 100755 index 0000000..f27907b --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..0f11e5d --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_expensive_turn_in_place_highcost.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..596f5c5 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_5cm_noreverse_trolley.mprim @@ -0,0 +1,2403 @@ +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 new file mode 100755 index 0000000..de16976 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_10cm.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..aa9670d --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_1cm.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..6bf962e --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2_5cm.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..b87c310 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_2cm.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100755 index 0000000..bff2688 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mprim/unicycle_highcost_5cm.mprim @@ -0,0 +1,1683 @@ +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 new file mode 100644 index 0000000..2f172c9 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/mqtt_general.yaml @@ -0,0 +1,8 @@ +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 new file mode 100644 index 0000000..e0431be --- /dev/null +++ b/Controllers/Packages/amr_startup/config/pnkx_local_planner_params.yaml @@ -0,0 +1,188 @@ + +position_planner_name: PNKXLocalPlanner +docking_planner_name: PNKXDockingLocalPlanner +go_straight_planner_name: PNKXGoStraightLocalPlanner +rotate_planner_name: PNKXRotateLocalPlanner + +base_local_planner: nav_core_adapter::LocalPlannerAdapter +yaw_goal_tolerance: 0.017 +xy_goal_tolerance: 0.015 +stateful: true +publish_topic: true + +LocalPlannerAdapter: + PNKXLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXDockingLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::PredictiveTrajectory + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXGoStraightLocalPlanner: + # Algorithm + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + algorithm_nav_name: mkt_algorithm::diff::GoStraight + # Goal checking + goal_checker_name: mkt_plugins::GoalChecker + + PNKXRotateLocalPlanner: + # Algorithm + algorithm_rotate_name: mkt_algorithm::diff::RotateToGoal + trajectory_generator_name: mkt_plugins::LimitedAccelGenerator + # Goal checking + goal_checker_name: mkt_plugins::SimpleGoalChecker + stateful: true + +LimitedAccelGenerator: + max_vel_x: 1.2 + min_vel_x: -1.2 + + max_vel_y: 0.0 # diff drive robot + min_vel_y: 0.0 # diff drive robot + + max_speed_xy: 2.0 # max_trans_vel: 0.8 # choose slightly less than the base's capability + min_speed_xy: 0.1 # min_trans_vel: 0.1 # this is the min trans velocity when there is negligible rotational velocity + + max_vel_theta: 0.3 # max_rot_vel: 1.0 # choose slightly less than the base's capability + min_vel_theta: 0.05 # min_rot_vel: 0.1 default: 0.4 # this is the min angular velocity when there is negligible translational velocity + + acc_lim_x: 1.0 + acc_lim_y: 0.0 # diff drive robot + acc_lim_theta: 0.5 + decel_lim_x: -2.0 + decel_lim_y: -0.0 + decel_lim_theta: -1.0 + + # Whether to split the path into segments or not + split_path: true + sim_time: 1.5 + vx_samples: 15 + vy_samples: 1 + vtheta_samples: 10 + discretize_by_time: true + angular_granularity: 0.05 + linear_granularity: 0.1 + # sim_period + include_last_point: true + +PredictiveTrajectory: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.5 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.8 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) + +GoStraight: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Minimum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.35 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + # Speed + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + use_regulated_linear_velocity_scaling: false + use_cost_regulated_linear_velocity_scaling: false + +RotateToGoal: + avoid_obstacles: false + xy_local_goal_tolerance: 0.01 + angle_threshold: 0.6 + index_samples: 60 + follow_step_path: true + + # Lookahead + use_velocity_scaled_lookahead_dist: true # Whether to use the velocity scaled lookahead distances or constant lookahead_distance. (default: false) + # only when false: + lookahead_dist: 0.325 # The lookahead distance (m) to use to find the lookahead point. (default: 0.6) + # only when true: + min_lookahead_dist: 0.5 # The minimum lookahead distance (m) threshold. (default: 0.3) + max_lookahead_dist: 1.5 # The maximum lookahead distance (m) threshold. (default: 0.9) + lookahead_time: 1.8 # The time (s) to project the velocity by, a.k.a. lookahead gain. (default: 1.5) + min_journey_squared: 0.3 # Minimum squared journey to consider for goal (default: 0.2) + max_journey_squared: 0.6 # Maximum squared journey to consider for goal (default: 0.2) + + # Rotate to heading param - onle one of use_rotate_to_heading and allow_reversing can be set to true + use_rotate_to_heading: true # Whether to enable rotating to rough heading and goal orientation when using holonomic planners. Recommended on for all robot types that can rotate in place. (default: true) + # only when true: + rotate_to_heading_min_angle: 0.03 # The difference in the path orientation and the starting robot orientation (radians) to trigger a rotate in place. (default: 0.785) + + # stoped + rot_stopped_velocity: 0.03 + trans_stopped_velocity: 0.06 + min_approach_linear_velocity: 0.05 # The minimum velocity (m/s) threshold to apply when approaching the goal to ensure progress. Must be > 0.01. (default: 0.05) + + # Regulated linear velocity scaling + use_regulated_linear_velocity_scaling: true # Whether to use the regulated features for path curvature (e.g. slow on high curvature paths). (default: true) + # only when true: + regulated_linear_scaling_min_radius: 0.6 # The turning radius (m) for which the regulation features are triggered. Remember, sharper turns have smaller radii. (default: 0.9) + regulated_linear_scaling_min_speed: 0.05 # The minimum speed (m/s) for which any of the regulated heuristics can send, to ensure process is still achievable even in high cost spaces with high curvature. Must be > 0.1. (default: 0.25) + + # Inflation cost scaling (Limit velocity by proximity to obstacles) + use_cost_regulated_linear_velocity_scaling: false # Whether to use the regulated features for proximity to obstacles (e.g. slow in close proximity to obstacles). (default: true) + inflation_cost_scaling_factor: 2.0 # (default: 3.0) # must be > 0 + cost_scaling_dist: 0.2 # (default: 0.6) + cost_scaling_gain: 2.0 # (default: 1.0) \ 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 new file mode 100755 index 0000000..cf1a0da --- /dev/null +++ b/Controllers/Packages/amr_startup/config/ros_diff_drive_controller.yaml @@ -0,0 +1,71 @@ +# ----------------------------------- +left_wheel : 'left_wheel_joint' +right_wheel : 'right_wheel_joint' +publish_rate: 50 # this is what the real MiR platform publishes (default: 50) +# These covariances are exactly what the real MiR platform publishes +pose_covariance_diagonal : [0.00001, 0.00001, 1000000.0, 1000000.0, 1000000.0, 1000.0] +twist_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 1000.0] +enable_odom_tf: true +enable_wheel_tf: true +allow_multiple_cmd_vel_publishers: true +# open_loop: false +# Wheel separation and diameter. These are both optional. +# diff_drive_controller will attempt to read either one or both from the +# URDF if not specified as a parameter. +# We don't set the value here because it's different for each MiR type (100, 250, ...), and +# the plugin figures out the correct values. +wheel_separation : 0.5106 +wheel_radius : 0.1 + +# Wheel separation and radius multipliers +wheel_separation_multiplier: 1.0 # default: 1.0 +wheel_radius_multiplier : 1.0 # default: 1.0 + +# Velocity commands timeout [s], default 0.5 +cmd_vel_timeout: 1.0 + +# frame_ids (same as real MiR platform) +base_frame_id: base_footprint # default: base_link base_footprint +odom_frame_id: odom # default: odom + +# Velocity and acceleration limits +# Whenever a min_* is unspecified, default to -max_* +linear: + x: + has_velocity_limits : true + max_velocity : 1.5 # m/s; move_base max_vel_x: 0.8 + min_velocity : -1.5 # m/s + has_acceleration_limits: true + max_acceleration : 3.0 # m/s^2; move_base acc_lim_x: 1.5 + min_acceleration : -3.0 # m/s^2 + has_jerk_limits : true + max_jerk : 5.0 # m/s^3 +angular: + z: + has_velocity_limits : true + max_velocity : 0.6 # rad/s; move_base max_rot_vel: 1.0 + min_velocity : -0.6 + has_acceleration_limits: true + max_acceleration : 3.0 # rad/s^2; move_base acc_lim_th: 2.0 + has_jerk_limits : true + max_jerk : 2.5 # rad/s^3 + +left_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: left_encoder + frame_id: left_wheel_link + wheel_topic: /left_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, 0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id + +right_wheel_joint: + lookup_name: WheelPlugin + max_publish_rate: 50 + mesurement_topic: right_encoder + frame_id: right_wheel_link + wheel_topic: /right_wheel + subscribe_queue_size: 1 + command_timeout: 5.0 + origin : [0.0, -0.255, 0.075, 0.0, 0.0, 0.0] # origin from base_frame_id diff --git a/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml b/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml new file mode 100755 index 0000000..ec2ac75 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/sbpl_global_params.yaml @@ -0,0 +1,10 @@ +base_global_planner: SBPLLatticePlanner +SBPLLatticePlanner: + environment_type: XYThetaLattice + planner_type: ARAPlanner + allocated_time: 10.0 + initial_epsilon: 1.0 + force_scratch_limit: 10000 + forward_search: true + nominalvel_mpersecs: 0.2 + timetoturn45degsinplace_secs: 0.6 # = 0.6 rad/s diff --git a/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml b/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml new file mode 100644 index 0000000..959a137 --- /dev/null +++ b/Controllers/Packages/amr_startup/config/sick_line_guidance_mls.yaml @@ -0,0 +1,61 @@ +bus: + device: can0 + driver_plugin: can::SocketCANInterface + master_allocator: canopen::SimpleMaster::Allocator +sync: + interval_ms: 10 + overflow: 0 +# +# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats) +# rate: 100 # heartbeat rate (1/rate in seconds) +# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started +nodes: + f_mlse: + id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "f_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "f_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # + + b_mlse: + id: 0x02 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS + eds_pkg: sick_line_guidance # package name for relative path + eds_file: mls/SICK-MLS.eds # path to EDS/DCF file + publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"] + # MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022 + + # sick_line_guidance configuration of this node: + sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported + sick_topic: "b_mlse" # MLS_Measurement messages are published in topic "/mls" + subscribe_queue_size: 1 + sick_frame_id: "b_mlse" # MLS_Measurement messages are published frame_id "mls_measurement_frame" + + # device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue" + # example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01 + # dcf_overlay: + # "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0 + # "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0 + # "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600 + # "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0 + # "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0 + # "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0 + # \ 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 new file mode 100644 index 0000000..ca0b60f --- /dev/null +++ b/Controllers/Packages/amr_startup/config/two_points_global_params.yaml @@ -0,0 +1,3 @@ +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 new file mode 100644 index 0000000..269a45a --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/amr_control.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + [[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 new file mode 100644 index 0000000..5a53679 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/amr_startup.launch @@ -0,0 +1,102 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ 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 new file mode 100755 index 0000000..51daad5 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/fake_detected_shelf.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/Controllers/Packages/amr_startup/launch/hector_mapping.launch b/Controllers/Packages/amr_startup/launch/hector_mapping.launch new file mode 100644 index 0000000..d74fa3e --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/hector_mapping.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ 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 new file mode 100644 index 0000000..3569a2c --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/amcl.launch @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml b/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml new file mode 100644 index 0000000..1b77cca --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/ekf.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/rviz.launch b/Controllers/Packages/amr_startup/launch/includes/rviz.launch new file mode 100644 index 0000000..b415953 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/rviz.launch @@ -0,0 +1,3 @@ + + + \ 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 new file mode 100644 index 0000000..0c0063a --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/spawn_maze.launch.xml @@ -0,0 +1,5 @@ + + + + diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch new file mode 100644 index 0000000..b223b41 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch @@ -0,0 +1,11 @@ + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/robot_empty_world.launch b/Controllers/Packages/amr_startup/launch/robot_empty_world.launch new file mode 100644 index 0000000..af3ae07 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_empty_world.launch @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch b/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch new file mode 100644 index 0000000..3efd0c4 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_gazebo_common.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [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 new file mode 100644 index 0000000..7569152 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/robot_maze_world.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/sehc_world.launch b/Controllers/Packages/amr_startup/launch/sehc_world.launch new file mode 100644 index 0000000..1e35cc4 --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/sehc_world.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ 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 new file mode 100644 index 0000000..aacc94c --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/start_maps.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/Controllers/Packages/amr_startup/launch/ware_houses_world.launch b/Controllers/Packages/amr_startup/launch/ware_houses_world.launch new file mode 100644 index 0000000..b05446f --- /dev/null +++ b/Controllers/Packages/amr_startup/launch/ware_houses_world.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/package.xml b/Controllers/Packages/amr_startup/package.xml new file mode 100644 index 0000000..0413dec --- /dev/null +++ b/Controllers/Packages/amr_startup/package.xml @@ -0,0 +1,59 @@ + + + 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 new file mode 100644 index 0000000..7c0221b --- /dev/null +++ b/Controllers/Packages/amr_startup/rosconsole.config @@ -0,0 +1,2 @@ +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 new file mode 100644 index 0000000..1f16975 --- /dev/null +++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz @@ -0,0 +1,646 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Grid1/Offset1 + - /TF1/Frames1 + - /Global Map1/Straight Path1 + - /Local Map1 + - /Local Map1/Trajectory1 + - /Local Map1/Trajectory1/transform plan1 + - /Pose1 + Splitter Ratio: 0.37295082211494446 + Tree Height: 422 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: Back LaserScan +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 5 + Y: 5 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: map + Value: true + - Alpha: 0.4000000059604645 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 0.5 + Class: rviz/RobotModel + Collision Enabled: true + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + back_nanoscan3_base_link: + Value: false + back_nanoscan3_sensor_link: + Value: false + base_footprint: + Value: true + base_link: + Value: false + bl_caster_rotation_link: + Value: false + bl_caster_wheel_link: + Value: false + br_caster_rotation_link: + Value: false + br_caster_wheel_link: + Value: false + camera_link: + Value: false + fl_caster_rotation_link: + Value: false + fl_caster_wheel_link: + Value: false + fr_caster_rotation_link: + Value: false + fr_caster_wheel_link: + Value: false + front_nanoscan3_base_link: + Value: false + front_nanoscan3_sensor_link: + Value: false + imu_frame: + Value: false + imu_link: + Value: false + left_wheel_link: + Value: false + lifting_link: + Value: false + map: + Value: true + odom: + Value: true + right_wheel_link: + Value: false + surface: + Value: false + Marker Alpha: 1 + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + back_nanoscan3_base_link: + back_nanoscan3_sensor_link: + {} + bl_caster_rotation_link: + bl_caster_wheel_link: + {} + br_caster_rotation_link: + br_caster_wheel_link: + {} + camera_link: + {} + fl_caster_rotation_link: + fl_caster_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_wheel_link: + {} + front_nanoscan3_base_link: + front_nanoscan3_sensor_link: + {} + imu_link: + imu_frame: + {} + left_wheel_link: + {} + lifting_link: + surface: + {} + right_wheel_link: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 204; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Front LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 6 + Size (m): 0.09000000357627869 + Style: Points + Topic: f_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 0; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: Back LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: /b_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Arrow Length: 0.20000000298023224 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz/PoseArray + Color: 0; 192; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Amcl Particle Swarm + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: particlecloud + Unreliable: false + Value: false + - Class: rviz/Group + Displays: + - Alpha: 0.5 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: true + Enabled: true + Name: Costmap + Topic: /amr_node/global_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.20000000298023224 + Line Style: Lines + Line Width: 0.05000000074505806 + Name: Full Plan + Offset: + X: 0 + Y: 0 + Z: 0.8999999761581421 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.019999999552965164 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/SBPLLatticePlanner/plan + Unreliable: false + Value: false + - Alpha: 1 + Class: rviz/Polygon + Color: 255; 0; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: move_base_node/global_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.00800000037997961 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: CustomPath + Offset: + X: 0 + Y: 0 + Z: 0.8999999761581421 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.004999999888241291 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/CustomPlanner/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Straight Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan + Unreliable: false + Value: false + Enabled: true + Name: Global Map + - Class: rviz/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 252; 175; 62 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.03999999910593033 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Global Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/global_plan + Unreliable: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Costmap + Topic: /amr_node/local_costmap/costmap + Unreliable: false + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Footprint + Queue Size: 10 + Topic: /amr_node/local_costmap/footprint + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 138; 226; 52 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Axes + Queue Size: 10 + Radius: 0.009999999776482582 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan + Unreliable: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /amr_node/PredictiveTrajectory/cost_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Group + Displays: + - Alpha: 1 + Axes Length: 0.10000000149011612 + Axes Radius: 0.029999999329447746 + Class: rviz/Pose + Color: 0; 0; 0 + Enabled: true + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: SubGoalPose + Queue Size: 10 + Shaft Length: 0.029999999329447746 + Shaft Radius: 0.029999999329447746 + Shape: Arrow + Topic: /amr_node/sub_goal + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 0.30000001192092896 + Axes Radius: 0.10000000149011612 + Class: rviz/Pose + Color: 252; 233; 79 + Enabled: false + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: ClosetPose + Queue Size: 10 + Shaft Length: 0.029999999329447746 + Shaft Radius: 0.029999999329447746 + Shape: Arrow + Topic: /amr_node/closet_robot_goal + Unreliable: false + Value: false + - Alpha: 1 + Axes Length: 0.05000000074505806 + Axes Radius: 0.019999999552965164 + Class: rviz/Pose + Color: 173; 127; 168 + Enabled: false + Head Length: 0.05000000074505806 + Head Radius: 0.05000000074505806 + Name: Look A Head Pose + Queue Size: 10 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /amr_node/lookahead_point + Unreliable: false + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.029999999329447746 + Class: rviz/PoseArray + Color: 0; 0; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.05000000074505806 + Name: reached intermediated goals + Queue Size: 10 + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.029999999329447746 + Shape: Axes + Topic: /amr_node/reached_intermediate_goals + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 245; 121; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.019999999552965164 + Length: 0.029999999329447746 + Line Style: Billboards + Line Width: 0.009999999776482582 + Name: transform plan + Offset: + X: 0 + Y: 0 + Z: 0.5 + Pose Color: 138; 226; 52 + Pose Style: Axes + Queue Size: 10 + Radius: 0.019999999552965164 + Shaft Diameter: 0.009999999776482582 + Shaft Length: 0.009999999776482582 + Topic: /amr_node/transformed_plan + Unreliable: false + Value: true + - Class: rviz/Marker + Enabled: false + Marker Topic: /amr_node/PredictiveTrajectory/cost_left_goals + Name: L + Namespaces: + {} + Queue Size: 100 + Value: false + - Class: rviz/Marker + Enabled: false + Marker Topic: /amr_node/PredictiveTrajectory/cost_right_goals + Name: R + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Name: Trajectory + Enabled: true + Name: Local Map + - Alpha: 1 + Axes Length: 0.05000000074505806 + Axes Radius: 0.014999999664723873 + Class: rviz/Pose + Color: 46; 52; 54 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /amr_node/current_goal + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/MoveCamera + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/Select + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: move_base_simple/goal + - Class: rviz/Measure + - Class: rviz/PublishPoint + Single click: true + Topic: clicked_point + Value: true + Views: + Current: + Angle: -3.135005474090576 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: -359.3964538574219 + Target Frame: base_link + X: -0.49439820647239685 + Y: 0.196189746260643 + Saved: + - Angle: -34.55989074707031 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: -132.97349548339844 + Target Frame: base_link + X: 34.338645935058594 + Y: 35.28913879394531 +Window Geometry: + Displays: + collapsed: false + Height: 573 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1048 + X: 0 + Y: 21 diff --git a/Controllers/Packages/amr_startup/sdf/maze/model.config b/Controllers/Packages/amr_startup/sdf/maze/model.config new file mode 100644 index 0000000..9a00866 --- /dev/null +++ b/Controllers/Packages/amr_startup/sdf/maze/model.config @@ -0,0 +1,11 @@ + + + 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 new file mode 100644 index 0000000..b31cc0b --- /dev/null +++ b/Controllers/Packages/amr_startup/sdf/maze/model.sdf @@ -0,0 +1,345 @@ + + + + -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/pnkx_nav_core b/pnkx_nav_core index 11b7c4a..ca9e100 160000 --- a/pnkx_nav_core +++ b/pnkx_nav_core @@ -1 +1 @@ -Subproject commit 11b7c4a20de070382728f8151b93528cc903d1c4 +Subproject commit ca9e100bd91c94975480244be32e6ee0a3fd0f92