git commit -m "first commit for v2"
This commit is contained in:
243
Controllers/Packages/amr_control/CMakeLists.txt
Normal file
243
Controllers/Packages/amr_control/CMakeLists.txt
Normal file
@@ -0,0 +1,243 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(amr_control)
|
||||
|
||||
## Compile as C++11, supported in ROS Kinetic and newer
|
||||
add_compile_options(-std=c++17)
|
||||
|
||||
## 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 COMPONENTS
|
||||
geometry_msgs
|
||||
nav_2d_utils
|
||||
roscpp
|
||||
rospy
|
||||
std_msgs
|
||||
angles
|
||||
delta_modbus
|
||||
nova5_control
|
||||
loc_core
|
||||
move_base_core
|
||||
amr_comunication
|
||||
vda5050_msgs
|
||||
)
|
||||
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
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 ##
|
||||
###################################
|
||||
## 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_control
|
||||
CATKIN_DEPENDS geometry_msgs move_base_core loc_core nav_2d_utils roscpp rospy std_msgs
|
||||
DEPENDS Boost
|
||||
)
|
||||
|
||||
###########
|
||||
## Build ##
|
||||
###########
|
||||
|
||||
## Specify additional locations of header files
|
||||
## Your package locations should be listed before other locations
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
## Declare a C++ library
|
||||
add_library(${PROJECT_NAME}
|
||||
src/amr_control.cpp
|
||||
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
|
||||
)
|
||||
|
||||
## 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_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})
|
||||
|
||||
## 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(vda_5050_api_test
|
||||
${PROJECT_NAME}
|
||||
${${PROJECT_NAME}_EXPORTED_TARGETS}
|
||||
${catkin_EXPORTED_TARGETS}
|
||||
)
|
||||
|
||||
## Specify libraries to link a library or executable target against
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
${FreeOpcUa_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}_node
|
||||
${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
target_link_libraries(vda_5050_api_test
|
||||
${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
|
||||
#############
|
||||
## 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 vda_5050_api_test
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
## Mark libraries for installation
|
||||
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
|
||||
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.)
|
||||
#############
|
||||
## Testing ##
|
||||
#############
|
||||
|
||||
## Add gtest based cpp test target and link libraries
|
||||
# catkin_add_gtest(${PROJECT_NAME}-test test/test_amr_control.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)
|
||||
@@ -0,0 +1,122 @@
|
||||
#ifndef __AMR_COTROLLER_H_INCLUDED_
|
||||
#define __AMR_COTROLLER_H_INCLUDED_
|
||||
#include <ros/ros.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <condition_variable>
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
|
||||
#include "delta_modbus/delta_modbus_tcp.h"
|
||||
|
||||
// API
|
||||
#include "amr_control/amr_opc_ua_server_api.h"
|
||||
#include "amr_control/amr_vda_5050_client_api.h"
|
||||
|
||||
// Objects
|
||||
#include "move_base_core/navigation.h"
|
||||
#include "loc_core/localization.h"
|
||||
#include "amr_control/amr_monitor.h"
|
||||
#include "nova5_control/imr_nova_control.h"
|
||||
#include "amr_control/amr_safety.h"
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
class AmrController
|
||||
{
|
||||
public:
|
||||
AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
|
||||
AmrController();
|
||||
virtual ~AmrController();
|
||||
|
||||
void init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer);
|
||||
|
||||
/**
|
||||
* @brief Stop the ServerHandle
|
||||
*/
|
||||
virtual void stop(void);
|
||||
|
||||
protected:
|
||||
virtual void initalizingComunicationHandle(ros::NodeHandle &nh);
|
||||
virtual void initalizingMoveBaseHandle(ros::NodeHandle &nh);
|
||||
virtual void initalizingLocalizationHandle(ros::NodeHandle &nh);
|
||||
virtual void initalizingMonitorHandle(ros::NodeHandle &nh);
|
||||
|
||||
private:
|
||||
void ArmCallBack();
|
||||
|
||||
void ArmDotuff();
|
||||
|
||||
void unLoadCallBack();
|
||||
|
||||
void conveyorBeltsShipping(amr_control::State &state);
|
||||
|
||||
void loadCallBack();
|
||||
|
||||
void conveyorBeltsReceiving(amr_control::State &state);
|
||||
|
||||
void controllerDotuff();
|
||||
|
||||
void isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg);
|
||||
|
||||
void threadHandle();
|
||||
|
||||
bool initalized_;
|
||||
ros::NodeHandle nh_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_;
|
||||
ros::Subscriber is_detected_maker_sub_;
|
||||
|
||||
std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
|
||||
std::shared_ptr<amr_control::VDA5050ClientAPI> vda_5050_client_api_ptr_;
|
||||
std::shared_ptr<std::thread> server_thr_;
|
||||
|
||||
std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
|
||||
std::shared_ptr<std::thread> monitor_thr_;
|
||||
|
||||
std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
|
||||
pluginlib::ClassLoader<move_base_core::BaseNavigation> move_base_loader_;
|
||||
// Synchronous processing thread
|
||||
std::shared_ptr<std::thread> move_base_thr_;
|
||||
std::mutex init_move_base_mutex_;
|
||||
std::condition_variable init_move_base_cv_;
|
||||
bool move_base_initialized_;
|
||||
|
||||
// Safety Speed
|
||||
std::shared_ptr<AmrSafety> amr_safety_ptr_;
|
||||
bool muted_;
|
||||
std::mutex cmd_vel_mtx;
|
||||
nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_recommended_;
|
||||
|
||||
std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
|
||||
pluginlib::ClassLoader<loc_core::BaseLocalization> loc_base_loader_;
|
||||
// Synchronous processing thread
|
||||
std::shared_ptr<std::thread> loc_base_thr_;
|
||||
std::mutex init_loc_base_mutex_;
|
||||
std::condition_variable init_loc_base_cv_;
|
||||
bool loc_base_initialized_;
|
||||
|
||||
bool cancel_, enable_ ,pause_;
|
||||
// Belt properties
|
||||
std::thread belt_thread_;
|
||||
amr_control::State cur_belt_state_en_;
|
||||
bool belt_joined_;
|
||||
std::mutex belt_mutex_;
|
||||
|
||||
// amr propertise
|
||||
std::thread arm_thread_;
|
||||
bool arm_joined_;
|
||||
std::mutex arm_mutex_;
|
||||
bool arm_continue_;
|
||||
bool arm_go_home_;
|
||||
bool arm_power_on_;
|
||||
unsigned int count_ng_max_, count_ok_max_;
|
||||
unsigned int *status_code_ptr_ = new unsigned int;
|
||||
|
||||
}; // class AMRController
|
||||
} // namespace amr_control
|
||||
|
||||
#endif // __AMR_COTROLLER_H_INCLUDED_
|
||||
@@ -0,0 +1,67 @@
|
||||
#ifndef _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
|
||||
#define _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
|
||||
#include <ros/ros.h>
|
||||
#include "amr_comunication/vda_5050/utils/curve_common.h"
|
||||
#include "amr_comunication/vda_5050/utils/pose.h"
|
||||
#include <amr_comunication/vda_5050/utils/common.h>
|
||||
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
inline double getYawDegree(double x, double y, double z, double w) {
|
||||
// yaw (z-axis rotation)
|
||||
double siny_cosp = 2 * (w * z + x * y);
|
||||
double cosy_cosp = 1 - 2 * (y * y + z * z);
|
||||
double yaw = std::atan2(siny_cosp, cosy_cosp);
|
||||
yaw = (180 * yaw) / M_PI;return yaw;
|
||||
}
|
||||
|
||||
inline double getYawRad(double x, double y, double z, double w) {
|
||||
// yaw (z-axis rotation)
|
||||
double siny_cosp = 2 * (w * z + x * y);
|
||||
double cosy_cosp = 1 - 2 * (y * y + z * z);
|
||||
double yaw = std::atan2(siny_cosp, cosy_cosp);
|
||||
return yaw;
|
||||
}
|
||||
|
||||
inline double calculateAngle(double xA, double yA, double xB, double yB) {
|
||||
double deltaX = xB - xA;
|
||||
double deltaY = yB - yA;
|
||||
double angleRad = atan2(deltaY, deltaX);
|
||||
// double angleDeg = angleRad * 180.0 / M_PI;
|
||||
return angleRad;
|
||||
}
|
||||
|
||||
inline void modifyYaw(double& yaw) {
|
||||
while (yaw < -M_PI)
|
||||
yaw += 2.0 * M_PI;
|
||||
while (yaw > M_PI)
|
||||
yaw -= 2.0 * M_PI;
|
||||
};
|
||||
|
||||
double calculateDistance(vda_5050::Pose& pose1, vda_5050::Pose& pose2);
|
||||
|
||||
double computeDeltaAngle(vda_5050::Pose& Pose1, vda_5050::Pose& Pose2);
|
||||
|
||||
bool isThetaValid(double theta);
|
||||
|
||||
bool curveIsValid(int degree, const std::vector<double>& knot_vector, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>& control_points);
|
||||
|
||||
double computeDeltaAngleStartNode(double theta, vda_5050::Pose& startPose, vda_5050::Pose& next_Pose);
|
||||
|
||||
double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose& Pose);
|
||||
|
||||
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose& startPose, geometry_msgs::Pose& next_Pose);
|
||||
|
||||
double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose& Pose);
|
||||
|
||||
double computeDeltaAngleEndNode(double theta, vda_5050::Pose& endPose, vda_5050::Pose& prev_Pose);
|
||||
|
||||
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose& endPose, geometry_msgs::Pose& prev_Pose);
|
||||
|
||||
void setYawAllPosesOnEdge(std::vector<vda_5050::Pose>& posesOnEdge, bool reverse);
|
||||
|
||||
bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t& status, std::string& message, std::vector<vda_5050::Pose>& posesOnPathWay);
|
||||
}
|
||||
|
||||
#endif // _VDA_5050_MAKE_PLAN_WITH_ORDER_H_INCLUDE_
|
||||
@@ -0,0 +1,49 @@
|
||||
#ifndef __AMR_MONITOR_H_INCLUDED_
|
||||
#define __AMR_MONITOR_H_INCLUDED_
|
||||
#include <ros/ros.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <nav_2d_utils/odom_subscriber.h>
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
class AmrMonitor
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Default constructor for AmrMonitor
|
||||
*/
|
||||
AmrMonitor();
|
||||
|
||||
/**
|
||||
* @brief Constructor for AmrMonitor with ROS node handle
|
||||
* @param nh ROS NodeHandle for initialization
|
||||
*/
|
||||
AmrMonitor(const ros::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor for AmrMonitor
|
||||
*/
|
||||
virtual ~AmrMonitor();
|
||||
|
||||
/**
|
||||
* @brief Initialize the ServerHandle with a ROS node handle
|
||||
* @param nh ROS NodeHandle for initialization
|
||||
*/
|
||||
virtual void init(const ros::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Get velocity
|
||||
* @return True if get done
|
||||
*/
|
||||
bool getVelocity(nav_2d_msgs::Twist2D &velocity);
|
||||
|
||||
private:
|
||||
|
||||
std::shared_ptr <nav_2d_utils::OdomSubscriber> odom_sub_;
|
||||
bool initalized_;
|
||||
};
|
||||
} // namespace amr_control
|
||||
|
||||
#endif // __AMR_MONITOR_H_INCLUDED_
|
||||
@@ -0,0 +1,912 @@
|
||||
#ifndef __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_
|
||||
#define __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "move_base_core/navigation.h"
|
||||
#include "loc_core/localization.h"
|
||||
#include "amr_comunication/opc_ua/ua_server.h"
|
||||
#include "amr_control/amr_monitor.h"
|
||||
#include "amr_control/common.h"
|
||||
#include <pthread.h>
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
class OpcUAServerAPI
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Contructor
|
||||
* @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện.
|
||||
* @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot.
|
||||
* @param loc_base Con trỏ đến đối tượng LocBase.
|
||||
*/
|
||||
OpcUAServerAPI(const ros::NodeHandle &nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
|
||||
std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<amr_control::AmrMonitor> monitor);
|
||||
|
||||
/**
|
||||
* @brief Detructor
|
||||
*/
|
||||
virtual ~OpcUAServerAPI();
|
||||
|
||||
/**
|
||||
* @brief Start
|
||||
*/
|
||||
void start();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Định nghĩa các đối tượng cần thiết cho API người dùng.
|
||||
*
|
||||
* Hàm này cho phép người dùng định nghĩa và cấu hình các đối tượng cần thiết
|
||||
* cho việc tương tác với server, monitor và hệ thống điều hướng.
|
||||
* Người dùng có thể sử dụng các đối tượng này để thực hiện các hành động
|
||||
* như di chuyển, nhặt đồ, thả đồ, và các hành động khác liên quan đến robot.
|
||||
*/
|
||||
virtual void defineObjects();
|
||||
|
||||
/** SLAM**/
|
||||
/**
|
||||
* @brief Thêm phương thức bắt đầu quét map.
|
||||
* Bắt đầu quá trình tạo bản đồ. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
|
||||
* Khi gọi thành công thì trạng thái biến "SlamState" sẽ chuyển thành "Mapping".
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addStartMappingMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức bắt đầu quét map.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức dừng quét map.
|
||||
* Kết thúc quá trình tạo bản đồ, truyền vào tên map để lưu trữ.
|
||||
* Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready".
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addStopMappingMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức dừng quét map.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức bắt đầu định vị bản đồ.
|
||||
* Bắt đầu chế độ Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
|
||||
* Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Localization".
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addStartLocalizationMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức bắt đầu định vị bản đồ.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức kết thúc quá trình định vị.
|
||||
* Tắt chế độ Localization.Lúc này thông tin toạ độ robot sẽ không được cập nhật.
|
||||
* Nếu gọi thành công thì biến "SlamState" sẽ chuyển thành "Ready".
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addStopLocalizationMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức kết thúc quá trình định vị.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức liệt kê các file bản đồ.
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addListMapFilesMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức liệt kê các file bản đồ.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức chọn map file.
|
||||
* Chọn một map để Localization. Khi biến trạng thái "SlamState" đang là "Ready" thì có thể gọi được hàm này.
|
||||
*
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addActivateMapMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức chọn map file.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức khai báo vị trí vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addSetInitialPoseMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức khai báo vị trí
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm thuộc tính tọa độ của robot vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addRobotPoseProperty(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
static void slamHandle();
|
||||
|
||||
/** Navigation**/
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức di chuyển đến một nút cụ thể vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addMoveToNodeMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức di chuyển đến nút.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức di chuyển đến vị trí nhặt đồ vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addDockToNodeMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức nhặt đồ.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức quay vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addRotateToMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức quay.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức di chuyển thẳng vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addMoveStraightMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức di chuyển thẳng.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức hủy bỏ vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addCancelMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức hủy bỏ.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức tạm dừng vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addPauseMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức tạm dừng.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức tiếp tục vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addResumeMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức tiếp tục.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức reset vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addResetMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức reset.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức bật tắt vùng Muted vào server.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addWriteMutedMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức bật tắt vùng Muted.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm thuộc tính trạng thái của Robot khi di chuyển.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addNavigationState(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
static void navigationHandle();
|
||||
|
||||
/** Moritoring **/
|
||||
/**
|
||||
* @brief Thêm thuộc tính vận tốc của Robot khi di chuyển.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addVelocityCommand(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
static void monitorHandle();
|
||||
|
||||
/** IMR Plus **/
|
||||
/**
|
||||
* @brief Thêm phương thức khởi động và chạy tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addPickUpDobotMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức khởi động và chạy tay máy Dobot.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức về gốc tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addGoHomeDobotMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức về gốc tay máy Dobot.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thay đổi số lượng hàng cần lấy
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addCountMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức thay đổi số lượng hàng cần lấy
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức dừng tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addCancelDobotMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức dừng tay máy Dobot.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức tiếp tục chạy tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addContinueDobotMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức tiếp tục chạy tay máy Dobot.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức khởi động nguồn tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addPowerOnDobotMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức khởi động nguồn tay máy Dobot.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm thuộc tính tay máy Dobot.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addDobotProperties(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
static void dobotPropertiesHandle();
|
||||
/**
|
||||
* @brief Thêm phương thức trả hàng.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addunLoadMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức trả hàng.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức nhận hàng.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addLoadMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức nhận hàng.
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
|
||||
/**
|
||||
* @brief Thêm phương thức Reset Băng tải
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addResetConveyorBeltMethod(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
/**
|
||||
* @brief Callback cho phương thức Reset Băng tải
|
||||
* @param server Con trỏ đến server.
|
||||
* @param sessionId ID của phiên làm việc.
|
||||
* @param sessionContext Ngữ cảnh của phiên làm việc.
|
||||
* @param methodId ID của phương thức.
|
||||
* @param methodContext Ngữ cảnh của phương thức.
|
||||
* @param objectId ID của đối tượng.
|
||||
* @param objectContext Ngữ cảnh của đối tượng.
|
||||
* @param inputSize Kích thước của dữ liệu đầu vào.
|
||||
* @param input Dữ liệu đầu vào.
|
||||
* @param outputSize Kích thước của dữ liệu đầu ra.
|
||||
* @param output Dữ liệu đầu ra.
|
||||
* @return UA_StatusCode Trạng thái của callback.
|
||||
*/
|
||||
static UA_StatusCode 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);
|
||||
/**
|
||||
* @brief Thêm thuộc tính trạng thái băng tải.
|
||||
* @param server Con trỏ đến server mà phương thức sẽ được thêm vào.
|
||||
* @param parentID ID của nút cha mà phương thức sẽ được thêm vào.
|
||||
*/
|
||||
static void addConveyorBeltState(UA_Server *server, UA_NodeId parentID);
|
||||
|
||||
static void ConveyorBeltHandle();
|
||||
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
static ros::Publisher init_pub_;
|
||||
static pthread_t hThread_;
|
||||
static void *ThreadWorker(void *_);
|
||||
|
||||
static std::shared_ptr<amr_comunication::AmrOpcUAServer> server_ptr_;
|
||||
static std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
|
||||
static std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
|
||||
static std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
|
||||
|
||||
static UA_NodeId *robotPoseX_Id_;
|
||||
static UA_NodeId *robotPoseY_Id_;
|
||||
static UA_NodeId *robotPoseYaw_Id_;
|
||||
static UA_NodeId *currentActiveMap_Id_;
|
||||
static UA_NodeId *slamState_Id_;
|
||||
static UA_NodeId *workingDirectory_Id_;
|
||||
|
||||
static UA_String amr_feedback_str_;
|
||||
static UA_String amr_status_str_;
|
||||
static UA_UInt32 amr_status_en_;
|
||||
static UA_NodeId *amr_status_str_Id_;
|
||||
static UA_NodeId *amr_status_en_Id_;
|
||||
static UA_NodeId *amr_robotFeedBack_Id_;
|
||||
|
||||
static UA_NodeId *vx_Id_;
|
||||
static UA_NodeId *vy_Id_;
|
||||
static UA_NodeId *omega_Id_;
|
||||
|
||||
static UA_NodeId *arm_dobot_state_str_Id_;
|
||||
static UA_NodeId *arm_dobot_state_en_Id_;
|
||||
static UA_NodeId *arm_dobot_count_ok_max_Id_;
|
||||
static UA_NodeId *arm_dobot_count_ng_max_Id_;
|
||||
static UA_NodeId *arm_dobot_mode_Id_;
|
||||
static UA_NodeId *arm_dobot_status_code_Id_;
|
||||
|
||||
static UA_NodeId *belt_status_str_Id_;
|
||||
static UA_NodeId *belt_status_en_Id_;
|
||||
static UA_NodeId *have_goods_Id_;
|
||||
|
||||
public:
|
||||
static void resetState();
|
||||
|
||||
/**
|
||||
* @brief helper class for subscribing to odometry
|
||||
*/
|
||||
static nav_2d_msgs::Twist2D cmd_vel_max_;
|
||||
|
||||
/**
|
||||
* @brief Set muted value
|
||||
*/
|
||||
static bool *muted_value_;
|
||||
|
||||
static std::thread *arm_thread_ptr_;
|
||||
static std::function<void()> arm_function_ptr_;
|
||||
static amr_control::State arm_dobot_state_en_;
|
||||
static UA_String arm_dobot_state_str_;
|
||||
static unsigned int *count_ng_max_, *count_ok_max_;
|
||||
static bool *arm_cancel_;
|
||||
static bool *arm_continue_;
|
||||
static bool *arm_go_home_;
|
||||
static bool *arm_power_on_;
|
||||
static double *mode_ptr_;
|
||||
static unsigned int *status_code_ptr_;
|
||||
static unsigned int old_status_code_;
|
||||
|
||||
static amr_control::State *cur_belt_state_en_;
|
||||
static UA_String belt_state_str_;
|
||||
static UA_Boolean have_goods_;
|
||||
static bool *belt_cancel_;
|
||||
static std::function<void()> unLoad_excuted_;
|
||||
static std::function<void()> load_excuted_;
|
||||
static std::thread *belt_thread_ptr_;
|
||||
};
|
||||
} // namespace amr_control
|
||||
|
||||
#endif // __AMR_COTROLLER_OPC_UA_SERVER_DEFINE_API_H_INCLUDED_
|
||||
@@ -0,0 +1,55 @@
|
||||
#ifndef __AMR_SAFETY_H_INCLUDED_
|
||||
#define __AMR_SAFETY_H_INCLUDED_
|
||||
#include <ros/ros.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include "delta_modbus/delta_modbus_tcp.h"
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
class AmrSafety
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Default constructor for AmrSafety
|
||||
*/
|
||||
AmrSafety();
|
||||
|
||||
/**
|
||||
* @brief Constructor for AmrSafety with ROS node handle
|
||||
* @param nh ROS NodeHandle for initialization
|
||||
*/
|
||||
AmrSafety(const ros::NodeHandle &nh);
|
||||
|
||||
/**
|
||||
* @brief Virtual destructor for AmrSafety
|
||||
*/
|
||||
virtual ~AmrSafety();
|
||||
|
||||
/**
|
||||
* @brief Initialize the ServerHandle with a ROS node handle
|
||||
* @param nh ROS NodeHandle for initialization
|
||||
*/
|
||||
virtual void init(const ros::NodeHandle &nh);
|
||||
|
||||
/**
|
||||
* @brief Get object PLC controller
|
||||
* @param plc_controller_ptr A smart pointer of the PLC object
|
||||
*/
|
||||
virtual bool getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr);
|
||||
|
||||
/**
|
||||
* @brief Safety Hanlde
|
||||
*/
|
||||
virtual void safetyHandle(nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel);
|
||||
|
||||
virtual void writeMutesSafety(bool value);
|
||||
|
||||
private:
|
||||
bool initalized_;
|
||||
std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
|
||||
};
|
||||
} // namespace amr_control
|
||||
|
||||
#endif // __AMR_SAFETY_H_INCLUDED_
|
||||
@@ -0,0 +1,122 @@
|
||||
#ifndef __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
|
||||
#define __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include "amr_comunication/vda_5050/vda_5050_connector.h"
|
||||
#include "move_base_core/navigation.h"
|
||||
#include "loc_core/localization.h"
|
||||
#include "amr_control/amr_monitor.h"
|
||||
#include "amr_control/common.h"
|
||||
#include <nav_2d_utils/odom_subscriber.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
class VDA5050ClientAPI
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Contructor
|
||||
*/
|
||||
VDA5050ClientAPI();
|
||||
|
||||
/**
|
||||
* @brief Contructor
|
||||
* @param monitor Con trỏ đến đối tượng AmrMonitor, dùng để theo dõi trạng thái và sự kiện.
|
||||
* @param move_base Con trỏ đến đối tượng BaseNavigation, cho phép điều hướng robot.
|
||||
* @param loc_base Con trỏ đến đối tượng LocBase.
|
||||
*/
|
||||
VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
|
||||
std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<amr_control::AmrMonitor> monitor);
|
||||
|
||||
/**
|
||||
* @brief Detructor
|
||||
*/
|
||||
virtual ~VDA5050ClientAPI();
|
||||
|
||||
protected:
|
||||
virtual void orderWorker();
|
||||
|
||||
virtual void moveTo(vda_5050::Order order, uint8_t &status, std::string &message);
|
||||
|
||||
virtual void moveToDock(vda_5050::Order order, uint8_t &status, std::string &message);
|
||||
|
||||
virtual void rotateTo(vda_5050::Order order, uint8_t &status, std::string &message);
|
||||
|
||||
virtual void instantActionWorker();
|
||||
|
||||
virtual void excuteAction();
|
||||
|
||||
virtual void updateVelocity(double velocity);
|
||||
|
||||
virtual void updateAngular(double angular);
|
||||
|
||||
virtual void cancelOrderCallBack(std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void cancelOrder(std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
template <typename T>
|
||||
void excuteNoneAction(
|
||||
void (T::*excute)(std::shared_ptr<vda_5050::Action>, std::shared_ptr<vda_5050::ActionState>), T *obj,
|
||||
std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void mutedSafetyON(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
virtual void mutedSafetyOFF(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state);
|
||||
|
||||
void updating();
|
||||
|
||||
void updatingvda5050Visualization();
|
||||
|
||||
void updatingvda5050State();
|
||||
|
||||
void resetState();
|
||||
|
||||
// properties
|
||||
std::shared_ptr<amr_comunication::VDA5050Connector> client_ptr_;
|
||||
static std::shared_ptr<move_base_core::BaseNavigation> move_base_ptr_;
|
||||
static std::shared_ptr<loc_core::BaseLocalization> loc_base_ptr_;
|
||||
static std::shared_ptr<amr_control::AmrMonitor> monitor_ptr_;
|
||||
|
||||
ros::Publisher plan_pub_;
|
||||
std::string global_plan_msg_type_;
|
||||
std::thread update_thread_;
|
||||
|
||||
std::map<std::string, std::thread> threads_map_;
|
||||
std::mutex threads_mutex;
|
||||
|
||||
public:
|
||||
// Belt Convey
|
||||
amr_control::State *cur_belt_state_en_;
|
||||
std::function<void()> unLoad_excuted_;
|
||||
std::function<void()> load_excuted_;
|
||||
std::thread *belt_thread_ptr_;
|
||||
std::mutex belt_mutex_;
|
||||
bool belt_joined_;
|
||||
|
||||
// arm control
|
||||
std::thread *arm_thread_ptr_;
|
||||
std::function<void()> arm_function_ptr_;
|
||||
unsigned int *count_ng_max_, *count_ok_max_;
|
||||
|
||||
bool *muted_value_;
|
||||
bool *pause_action_;
|
||||
bool *cancel_action_;
|
||||
bool *enable_action_;
|
||||
vda_5050::State::OperatingMode mode_;
|
||||
nav_2d_msgs::Twist2D cmd_vel_max_, cmd_vel_max_saved_;
|
||||
};
|
||||
|
||||
} // namespace amr_control
|
||||
|
||||
#endif // __AMR_COTROLLER_VDA_5050_CLIENT_DEFINE_API_H_INCLUDED_
|
||||
@@ -0,0 +1,25 @@
|
||||
#ifndef _AMR_CONTROL_COMMON_H_INCLUDED_
|
||||
#define _AMR_CONTROL_COMMON_H_INCLUDED_
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
enum State
|
||||
{
|
||||
WAITING,
|
||||
INITIALIZING,
|
||||
RUNNING,
|
||||
PAUSED,
|
||||
FINISHED,
|
||||
FAILED
|
||||
};
|
||||
|
||||
enum OperatingMode
|
||||
{
|
||||
AUTOMATIC,
|
||||
SEMIAUTOMATIC,
|
||||
MANUAL,
|
||||
SERVICE,
|
||||
TEACHING
|
||||
};
|
||||
}
|
||||
#endif
|
||||
96
Controllers/Packages/amr_control/package.xml
Normal file
96
Controllers/Packages/amr_control/package.xml
Normal file
@@ -0,0 +1,96 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>amr_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The amr_control package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="robotics@todo.todo">robotics</maintainer>
|
||||
|
||||
|
||||
<!-- One license tag required, multiple allowed, one license per tag -->
|
||||
<!-- Commonly used license strings: -->
|
||||
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
|
||||
<license>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/amr_control</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
|
||||
<!-- <depend>roscpp</depend> -->
|
||||
<!-- Note that this is equivalent to the following: -->
|
||||
<!-- <build_depend>roscpp</build_depend> -->
|
||||
<!-- <exec_depend>roscpp</exec_depend> -->
|
||||
<!-- Use build_depend for packages you need at compile time: -->
|
||||
<!-- <build_depend>message_generation</build_depend> -->
|
||||
<!-- Use build_export_depend for packages you need in order to build against this package: -->
|
||||
<!-- <build_export_depend>message_generation</build_export_depend> -->
|
||||
<!-- Use buildtool_depend for build tool packages: -->
|
||||
<!-- <buildtool_depend>catkin</buildtool_depend> -->
|
||||
<!-- Use exec_depend for packages you need at runtime: -->
|
||||
<!-- <exec_depend>message_runtime</exec_depend> -->
|
||||
<!-- Use test_depend for packages you need only for testing: -->
|
||||
<!-- <test_depend>gtest</test_depend> -->
|
||||
<!-- Use doc_depend for packages you need only for building documentation: -->
|
||||
<!-- <doc_depend>doxygen</doc_depend> -->
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>geometry_msgs</build_depend>
|
||||
<build_depend>move_base_core</build_depend>
|
||||
<build_depend>nav_2d_utils</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_depend>angles</build_depend>
|
||||
<build_depend>delta_modbus</build_depend>
|
||||
<build_depend>nova5_control</build_depend>
|
||||
<build_depend>loc_core</build_depend>
|
||||
<build_depend>amr_comunication</build_depend>
|
||||
<build_depend>vda5050_msgs</build_depend>
|
||||
|
||||
<build_export_depend>geometry_msgs</build_export_depend>
|
||||
<build_export_depend>move_base_core</build_export_depend>
|
||||
<build_export_depend>nav_2d_utils</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<build_export_depend>angles</build_export_depend>
|
||||
<build_export_depend>delta_modbus</build_export_depend>
|
||||
<build_export_depend>nova5_control</build_export_depend>
|
||||
<build_export_depend>loc_core</build_export_depend>
|
||||
<build_export_depend>amr_comunication</build_export_depend>
|
||||
<build_export_depend>vda5050_msgs</build_export_depend>
|
||||
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>move_base_core</exec_depend>
|
||||
<exec_depend>nav_2d_utils</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>angles</exec_depend>
|
||||
<exec_depend>delta_modbus</exec_depend>
|
||||
<exec_depend>nova5_control</exec_depend>
|
||||
<exec_depend>loc_core</exec_depend>
|
||||
<exec_depend>amr_comunication</exec_depend>
|
||||
<exec_depend>vda5050_msgs</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
578
Controllers/Packages/amr_control/src/amr_control.cpp
Normal file
578
Controllers/Packages/amr_control/src/amr_control.cpp
Normal file
@@ -0,0 +1,578 @@
|
||||
#include "amr_control/amr_control.h"
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
AmrController::AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
|
||||
: initalized_(false),
|
||||
vda_5050_client_api_ptr_(nullptr),
|
||||
opc_ua_server_api_ptr_(nullptr),
|
||||
move_base_initialized_(false),
|
||||
loc_base_initialized_(false),
|
||||
monitor_ptr_(nullptr),
|
||||
loc_base_ptr_(nullptr),
|
||||
move_base_ptr_(nullptr),
|
||||
amr_safety_ptr_(nullptr),
|
||||
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
|
||||
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
|
||||
{
|
||||
this->init(nh, buffer);
|
||||
}
|
||||
|
||||
AmrController::AmrController()
|
||||
: initalized_(false),
|
||||
move_base_initialized_(false),
|
||||
loc_base_initialized_(false),
|
||||
vda_5050_client_api_ptr_(nullptr),
|
||||
opc_ua_server_api_ptr_(nullptr),
|
||||
monitor_ptr_(nullptr),
|
||||
loc_base_ptr_(nullptr),
|
||||
move_base_ptr_(nullptr),
|
||||
amr_safety_ptr_(nullptr),
|
||||
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
|
||||
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
|
||||
{
|
||||
}
|
||||
|
||||
AmrController::~AmrController()
|
||||
{
|
||||
// Wait for both threads to complete
|
||||
if (move_base_thr_)
|
||||
{
|
||||
move_base_thr_->join();
|
||||
move_base_thr_.reset();
|
||||
}
|
||||
|
||||
if (loc_base_thr_)
|
||||
{
|
||||
loc_base_thr_->join();
|
||||
loc_base_thr_.reset();
|
||||
}
|
||||
|
||||
if (server_thr_)
|
||||
{
|
||||
server_thr_->join();
|
||||
server_thr_.reset();
|
||||
}
|
||||
|
||||
if (monitor_thr_)
|
||||
{
|
||||
monitor_thr_->join();
|
||||
monitor_thr_.reset();
|
||||
}
|
||||
|
||||
if (opc_ua_server_api_ptr_)
|
||||
{
|
||||
opc_ua_server_api_ptr_.reset();
|
||||
}
|
||||
|
||||
if (vda_5050_client_api_ptr_)
|
||||
{
|
||||
vda_5050_client_api_ptr_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
|
||||
{
|
||||
if (!initalized_)
|
||||
{
|
||||
nh_ = nh;
|
||||
tf_ = buffer;
|
||||
|
||||
monitor_thr_ = std::make_shared<std::thread>(
|
||||
[this]()
|
||||
{
|
||||
this->initalizingMonitorHandle(nh_);
|
||||
this->threadHandle();
|
||||
});
|
||||
|
||||
move_base_initialized_ = false;
|
||||
// Setup base localization
|
||||
loc_base_thr_ = std::make_shared<std::thread>(
|
||||
[this]()
|
||||
{
|
||||
this->initalizingLocalizationHandle(nh_);
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
|
||||
loc_base_initialized_ = true;
|
||||
}
|
||||
init_loc_base_cv_.notify_one();
|
||||
});
|
||||
|
||||
// Setup base navigation
|
||||
move_base_thr_ = std::make_shared<std::thread>(
|
||||
[this]()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
|
||||
init_loc_base_cv_.wait(lock, [this]()
|
||||
{ return loc_base_initialized_; });
|
||||
|
||||
this->initalizingMoveBaseHandle(nh_);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(init_move_base_mutex_);
|
||||
move_base_initialized_ = true;
|
||||
}
|
||||
init_move_base_cv_.notify_one();
|
||||
|
||||
// this->controllerDotuff();
|
||||
});
|
||||
|
||||
// Setup server
|
||||
server_thr_ = std::make_shared<std::thread>(
|
||||
[this]()
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(init_move_base_mutex_);
|
||||
init_move_base_cv_.wait(lock, [this]()
|
||||
{ return move_base_initialized_; });
|
||||
this->initalizingComunicationHandle(nh_);
|
||||
});
|
||||
|
||||
ros::NodeHandle nh_core;
|
||||
is_detected_maker_sub_ = nh_core.subscribe("/is_detected_maker", 1, &AmrController::isDetectedMakerCallback, this);
|
||||
initalized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
void AmrController::stop()
|
||||
{
|
||||
}
|
||||
|
||||
void AmrController::initalizingComunicationHandle(ros::NodeHandle &nh)
|
||||
{
|
||||
|
||||
this->count_ng_max_ = this->count_ok_max_ = 1;
|
||||
std::shared_ptr opc_ua_server_thr = std::make_shared<std::thread>(
|
||||
[this, nh]()
|
||||
{
|
||||
opc_ua_server_api_ptr_ =
|
||||
std::make_shared<OpcUAServerAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
|
||||
|
||||
OpcUAServerAPI::muted_value_ = &this->muted_;
|
||||
OpcUAServerAPI::unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
|
||||
OpcUAServerAPI::load_excuted_ = std::bind(&AmrController::loadCallBack, this);
|
||||
OpcUAServerAPI::belt_thread_ptr_ = &this->belt_thread_;
|
||||
OpcUAServerAPI::cur_belt_state_en_ = &this->cur_belt_state_en_;
|
||||
OpcUAServerAPI::belt_cancel_ = &this->cancel_;
|
||||
|
||||
OpcUAServerAPI::arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
|
||||
OpcUAServerAPI::arm_thread_ptr_ = &this->arm_thread_;
|
||||
OpcUAServerAPI::arm_cancel_ = &this->enable_;
|
||||
OpcUAServerAPI::arm_go_home_ = &this->arm_go_home_;
|
||||
OpcUAServerAPI::arm_continue_ = &this->arm_continue_;
|
||||
OpcUAServerAPI::arm_power_on_ = &this->arm_power_on_;
|
||||
OpcUAServerAPI::count_ng_max_ = &this->count_ng_max_;
|
||||
OpcUAServerAPI::count_ok_max_ = &this->count_ok_max_;
|
||||
OpcUAServerAPI::status_code_ptr_ = this->status_code_ptr_;
|
||||
opc_ua_server_api_ptr_->start();
|
||||
});
|
||||
|
||||
std::shared_ptr vda_5050_client_thr = std::make_shared<std::thread>(
|
||||
[this, nh]()
|
||||
{
|
||||
vda_5050_client_api_ptr_ =
|
||||
std::make_shared<VDA5050ClientAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
|
||||
|
||||
vda_5050_client_api_ptr_->muted_value_ = &this->muted_;
|
||||
vda_5050_client_api_ptr_->unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
|
||||
vda_5050_client_api_ptr_->load_excuted_ = std::bind(&AmrController::loadCallBack, this);
|
||||
vda_5050_client_api_ptr_->cur_belt_state_en_ = &this->cur_belt_state_en_;
|
||||
vda_5050_client_api_ptr_->belt_thread_ptr_ = &this->belt_thread_;
|
||||
|
||||
vda_5050_client_api_ptr_->arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
|
||||
vda_5050_client_api_ptr_->arm_thread_ptr_ = &this->arm_thread_;
|
||||
vda_5050_client_api_ptr_->enable_action_ = &this->enable_;
|
||||
vda_5050_client_api_ptr_->cancel_action_ = &this->cancel_;
|
||||
vda_5050_client_api_ptr_->pause_action_ = &this->pause_;
|
||||
vda_5050_client_api_ptr_->count_ng_max_ = &this->count_ng_max_;
|
||||
vda_5050_client_api_ptr_->count_ok_max_ = &this->count_ok_max_;
|
||||
});
|
||||
ROS_INFO("Initalizing comunication is done");
|
||||
opc_ua_server_thr->join();
|
||||
vda_5050_client_thr->join();
|
||||
}
|
||||
|
||||
void AmrController::initalizingLocalizationHandle(ros::NodeHandle &nh)
|
||||
{
|
||||
std::string obj_name("loc_base::LocBase");
|
||||
if (tf_ == nullptr)
|
||||
throw std::runtime_error("tf2_ros::Buffer object is null");
|
||||
try
|
||||
{
|
||||
loc_base_ptr_ = loc_base_loader_.createUniqueInstance(obj_name);
|
||||
ros::NodeHandle node = ros::NodeHandle(nh, "global_costmap");
|
||||
loc_base_ptr_->initialize(node, tf_);
|
||||
}
|
||||
catch (pluginlib::PluginlibException &ex)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
|
||||
exit(1);
|
||||
}
|
||||
catch (std::exception &e)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
void AmrController::initalizingMoveBaseHandle(ros::NodeHandle &nh)
|
||||
{
|
||||
std::string obj_name("move_base::MoveBase");
|
||||
if (tf_ == nullptr)
|
||||
throw std::runtime_error("tf2_ros::Buffer object is null");
|
||||
try
|
||||
{
|
||||
move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name);
|
||||
ROS_INFO("Created object %s susseced", obj_name.c_str());
|
||||
move_base_ptr_->initialize(tf_);
|
||||
ros::Rate r(3);
|
||||
do
|
||||
{
|
||||
r.sleep();
|
||||
ros::spinOnce();
|
||||
} while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
|
||||
|
||||
if (move_base_ptr_ != nullptr &&
|
||||
move_base_ptr_->nav_feedback_ != nullptr &&
|
||||
move_base_ptr_->nav_feedback_->is_ready)
|
||||
{
|
||||
geometry_msgs::Vector3 linear;
|
||||
linear.x = 0.3;
|
||||
move_base_ptr_->setTwistLinear(linear);
|
||||
linear.x = -0.3;
|
||||
move_base_ptr_->setTwistLinear(linear);
|
||||
}
|
||||
}
|
||||
catch (pluginlib::PluginlibException &ex)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
|
||||
exit(1);
|
||||
}
|
||||
catch (std::exception &e)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
|
||||
{
|
||||
this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
|
||||
}
|
||||
|
||||
void AmrController::ArmCallBack()
|
||||
{
|
||||
ROS_INFO("Arm Calling");
|
||||
std::lock_guard<std::mutex> lock(this->arm_mutex_);
|
||||
{
|
||||
arm_joined_ = true;
|
||||
this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
|
||||
}
|
||||
}
|
||||
|
||||
void AmrController::ArmDotuff()
|
||||
{
|
||||
std::shared_ptr<imr_nova_control> arm_control_ptr;
|
||||
arm_control_ptr = std::make_shared<imr_nova_control>();
|
||||
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<unsigned int *>(&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();
|
||||
}
|
||||
|
||||
arm_control_ptr.reset();
|
||||
ROS_INFO("Arm Finished");
|
||||
std::lock_guard<std::mutex> lock(this->arm_mutex_);
|
||||
this->arm_joined_ = false;
|
||||
}
|
||||
|
||||
void AmrController::unLoadCallBack()
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
|
||||
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
|
||||
plc_controller_ptr_->connect();
|
||||
|
||||
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);
|
||||
|
||||
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 (cancel_ || !plc_controller_ptr_->checkConnected())
|
||||
state = amr_control::State::FAILED;
|
||||
|
||||
plc_controller_ptr_->close();
|
||||
}
|
||||
if (plc_controller_ptr_)
|
||||
plc_controller_ptr_.reset();
|
||||
|
||||
std::lock_guard<std::mutex> lock(this->arm_mutex_);
|
||||
this->belt_joined_ = false;
|
||||
}
|
||||
|
||||
void AmrController::loadCallBack()
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
|
||||
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
|
||||
plc_controller_ptr_->connect();
|
||||
|
||||
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);
|
||||
|
||||
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;
|
||||
|
||||
plc_controller_ptr_->close();
|
||||
}
|
||||
if (plc_controller_ptr_)
|
||||
plc_controller_ptr_.reset();
|
||||
|
||||
std::lock_guard<std::mutex> lock(this->arm_mutex_);
|
||||
this->belt_joined_ = false;
|
||||
}
|
||||
|
||||
void AmrController::controllerDotuff()
|
||||
{
|
||||
ros::Rate r(10);
|
||||
while (ros::ok())
|
||||
{
|
||||
std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
|
||||
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
|
||||
plc_controller_ptr_->connect();
|
||||
r.sleep();
|
||||
ros::spinOnce();
|
||||
|
||||
if (plc_controller_ptr_ == nullptr)
|
||||
continue;
|
||||
if (!plc_controller_ptr_->checkConnected())
|
||||
continue;
|
||||
|
||||
this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
|
||||
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();
|
||||
}
|
||||
|
||||
if (move_base_ptr_ != nullptr &&
|
||||
move_base_ptr_->nav_feedback_ != nullptr &&
|
||||
move_base_ptr_->nav_feedback_->is_ready)
|
||||
{
|
||||
if (velocity.x <= -0.01)
|
||||
this->amr_safety_ptr_->writeMutesSafety(true);
|
||||
else
|
||||
{
|
||||
this->amr_safety_ptr_->writeMutesSafety(this->muted_);
|
||||
}
|
||||
}
|
||||
|
||||
bool have_goods;
|
||||
int have_goods_regsister = 142;
|
||||
if (!OpcUAServerAPI::belt_cancel_)
|
||||
{
|
||||
plc_controller_ptr_->getM(have_goods_regsister, have_goods);
|
||||
OpcUAServerAPI::have_goods_ = have_goods;
|
||||
}
|
||||
else
|
||||
{
|
||||
plc_controller_ptr_->resetM(have_goods_regsister);
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
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<std::mutex> lock(this->arm_mutex_);
|
||||
{
|
||||
if (!this->arm_joined_)
|
||||
{
|
||||
this->arm_thread_.join();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (this->belt_thread_.joinable())
|
||||
{
|
||||
std::lock_guard<std::mutex> 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);
|
||||
|
||||
geometry_msgs::Vector3 linear;
|
||||
geometry_msgs::Vector3 angular;
|
||||
|
||||
cmd_vel_mtx.lock();
|
||||
linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
|
||||
angular.z = this->cmd_vel_max_.theta;
|
||||
cmd_vel_mtx.unlock();
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
27
Controllers/Packages/amr_control/src/amr_control_node.cpp
Normal file
27
Controllers/Packages/amr_control/src/amr_control_node.cpp
Normal file
@@ -0,0 +1,27 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include "amr_control/amr_control.h"
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "move_base_node");
|
||||
ros::start();
|
||||
ROS_INFO("ros::this_node::getName() : %s", ros::this_node::getName().c_str());
|
||||
ros::NodeHandle nh(ros::this_node::getName());
|
||||
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
|
||||
tf2_ros::TransformListener tf2(*buffer);
|
||||
try
|
||||
{
|
||||
std::shared_ptr<amr_control::AmrController> amr_ctr_;
|
||||
amr_ctr_ = std::make_shared<amr_control::AmrController>(nh, buffer);
|
||||
ros::spin();
|
||||
if(amr_ctr_ != nullptr)
|
||||
amr_ctr_->stop();
|
||||
amr_ctr_.reset();
|
||||
}
|
||||
catch (const std::exception& exc)
|
||||
{
|
||||
std::cout << exc.what() << std::endl;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,712 @@
|
||||
#include "amr_control/amr_make_plan_with_order.h"
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t &status, std::string &message, std::vector<vda_5050::Pose> &posesOnPathWay)
|
||||
{
|
||||
std::map<std::string, vda_5050::Node> orderNodes;
|
||||
Spline_Inf *input_spline_inf = new Spline_Inf();
|
||||
CurveCommon *curve_design = new CurveCommon();
|
||||
posesOnPathWay.clear();
|
||||
std::map<std::string, vda_5050::Node> map_str;
|
||||
if ((int)order.nodes.size() == 0 || (int)order.edges.size() == 0)
|
||||
{
|
||||
status = 1;
|
||||
message = "Nodes or Edges in Order is empty";
|
||||
return false;
|
||||
}
|
||||
for (auto &node : order.nodes)
|
||||
{
|
||||
orderNodes.insert({node.nodeId, node});
|
||||
}
|
||||
for (int i = 0; i < (int)order.edges.size(); i++)
|
||||
{
|
||||
auto start_nodeId_it = orderNodes.find(order.edges[i].startNodeId);
|
||||
auto end_nodeId_it = orderNodes.find(order.edges[i].endNodeId);
|
||||
|
||||
if (start_nodeId_it != orderNodes.end() && end_nodeId_it != orderNodes.end())
|
||||
{
|
||||
std::vector<vda_5050::Pose> posesOnEdge;
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> control_points;
|
||||
std::vector<double> knot_vector;
|
||||
std::vector<double> weight_vector;
|
||||
int degree = 0;
|
||||
int order_ = 0;
|
||||
control_points.reserve(order.edges[i].trajectory.controlPoints.size());
|
||||
knot_vector.reserve(order.edges[i].trajectory.knotVector.size());
|
||||
weight_vector.reserve(order.edges[i].trajectory.controlPoints.size());
|
||||
for (int j = 0; j < (int)order.edges[i].trajectory.controlPoints.size(); j++)
|
||||
{
|
||||
control_points.push_back(Eigen::Vector3d(order.edges[i].trajectory.controlPoints[j].x, order.edges[i].trajectory.controlPoints[j].y, 0));
|
||||
weight_vector.push_back(order.edges[i].trajectory.controlPoints[j].weight);
|
||||
}
|
||||
for (int k = 0; k < (int)order.edges[i].trajectory.knotVector.size(); k++)
|
||||
{
|
||||
knot_vector.push_back(order.edges[i].trajectory.knotVector[k]);
|
||||
}
|
||||
degree = (int)order.edges[i].trajectory.degree;
|
||||
if (curveIsValid(degree, knot_vector, control_points))
|
||||
{
|
||||
double step = 0.01;
|
||||
order_ = degree + 1;
|
||||
input_spline_inf->control_point.clear();
|
||||
input_spline_inf->knot_vector.clear();
|
||||
input_spline_inf->weight.clear();
|
||||
curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector);
|
||||
curve_design->ReadSplineInf(input_spline_inf, weight_vector, false);
|
||||
for (double u_test = 0; u_test <= 1; u_test += step)
|
||||
{
|
||||
geometry_msgs::Point curve_point;
|
||||
curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true);
|
||||
if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y))
|
||||
posesOnEdge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0));
|
||||
// ROS_INFO("curve_point: %f, %f at u: %f",curve_point.x, curve_point.y, u_test);
|
||||
}
|
||||
if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
|
||||
{
|
||||
// if startNode of this edge and start element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
|
||||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
|
||||
}
|
||||
// if endNode of this edge and end element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
|
||||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
|
||||
}
|
||||
if (!posesOnPathWay.empty())
|
||||
{
|
||||
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
}
|
||||
else if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (is_move_backward == false)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
}
|
||||
}
|
||||
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
|
||||
{
|
||||
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
|
||||
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
|
||||
{
|
||||
if (i != 1)
|
||||
{ // don't check angle of edge 1
|
||||
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
|
||||
{
|
||||
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
|
||||
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
|
||||
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else // posesOnPathWay is empty
|
||||
{
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
|
||||
}
|
||||
}
|
||||
else if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
|
||||
{
|
||||
// if startNode of this edge and start element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
|
||||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
|
||||
}
|
||||
// if endNode of this edge and end element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
|
||||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
|
||||
}
|
||||
if (i == ((int)order.edges.size() - 1))
|
||||
{
|
||||
if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 1.5707963268) // <= 90 degree
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
else
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) // <= 50 degree
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
else if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) // >= 130 degree
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
|
||||
{
|
||||
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
|
||||
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
|
||||
{
|
||||
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
|
||||
{
|
||||
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
|
||||
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else // posesOnPathWay is empty
|
||||
{
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
|
||||
}
|
||||
}
|
||||
else if (isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
|
||||
{
|
||||
// if startNode of this edge and start element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
|
||||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
|
||||
}
|
||||
// if endNode of this edge and end element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
|
||||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
|
||||
}
|
||||
if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) // <= 50 degree)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
|
||||
}
|
||||
else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) // >= 130 degree
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
|
||||
{
|
||||
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
|
||||
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
|
||||
{
|
||||
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
|
||||
{
|
||||
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
|
||||
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else // posesOnPathWay is empty
|
||||
{
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// if startNode of this edge and start element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
|
||||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
|
||||
}
|
||||
// if endNode of this edge and end element of posesOnEdge are the different point
|
||||
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
|
||||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
|
||||
{
|
||||
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
|
||||
}
|
||||
// DeltaAngleStart <= 50 degree and DeltaAngleEnd <= 50 degree
|
||||
if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626 &&
|
||||
computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, false);
|
||||
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
// DeltaAngleStart >= 130 degree and DeltaAngleEnd >= 130 degree
|
||||
else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276 &&
|
||||
computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276)
|
||||
{
|
||||
setYawAllPosesOnEdge(posesOnEdge, true);
|
||||
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
|
||||
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
|
||||
}
|
||||
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
|
||||
{
|
||||
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
|
||||
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
|
||||
{
|
||||
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
|
||||
{
|
||||
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
|
||||
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
|
||||
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
|
||||
status = 3;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else // posesOnPathWay is empty
|
||||
{
|
||||
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", order.edges[i].edgeId.c_str(),
|
||||
order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 2;
|
||||
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
|
||||
", endNode: " + order.edges[i].endNodeId.c_str() + " is invalid NURBS-curve";
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_WARN("Edge: %s not found startNodeId: %s or endNodeId: %s", order.edges[i].edgeId.c_str(),
|
||||
order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
|
||||
status = 1;
|
||||
message = "Edge: " + order.edges[i].edgeId + " not found startNodeId: " + order.edges[i].startNodeId.c_str() +
|
||||
" or endNodeId: " + order.edges[i].endNodeId.c_str();
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
// ROS_INFO("Finish to compute at Edge[%d]: %s", i, order.edges[i].edgeDescription.c_str());
|
||||
}
|
||||
|
||||
status = 0;
|
||||
message = "Success to make plan: StartNode: " + order.edges[0].startNodeId + ", EndNode: " + order.edges[order.edges.size() - 1].endNodeId;
|
||||
|
||||
if (input_spline_inf)
|
||||
delete (input_spline_inf);
|
||||
if (curve_design)
|
||||
delete (curve_design);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isThetaValid(double theta)
|
||||
{
|
||||
bool result = false;
|
||||
if (theta < -M_PI || theta > M_PI)
|
||||
result = false;
|
||||
else
|
||||
result = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
bool curveIsValid(int degree, const std::vector<double> &knot_vector,
|
||||
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &control_points)
|
||||
{
|
||||
if (degree < 1 || degree > 9)
|
||||
{
|
||||
ROS_WARN("degree is invalid value");
|
||||
return false;
|
||||
}
|
||||
if (!((knot_vector.size() - degree - 1) == control_points.size()))
|
||||
{
|
||||
ROS_WARN("relation between degree, number of knots, and number of control points is invalid");
|
||||
return false;
|
||||
}
|
||||
// if(std::is_sorted(knot_vector.begin(), knot_vector.end()))
|
||||
// {
|
||||
// ROS_WARN("knot vector is not monotonic");
|
||||
// return false;
|
||||
// }
|
||||
return true;
|
||||
}
|
||||
|
||||
double computeDeltaAngleStartNode(double theta, vda_5050::Pose &startPose, vda_5050::Pose &next_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(theta))
|
||||
{
|
||||
double xAB = next_Pose.getX() - startPose.getX();
|
||||
double yAB = next_Pose.getY() - startPose.getY();
|
||||
double d = sqrt(xAB * xAB + yAB * yAB);
|
||||
double xC = startPose.getX() + d * cos(theta);
|
||||
double yC = startPose.getY() + d * sin(theta);
|
||||
double xAC = xC - startPose.getX();
|
||||
double yAC = yC - startPose.getY();
|
||||
double dAB = sqrt(xAB * xAB + yAB * yAB);
|
||||
double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double computeDeltaAngleEndNode(double theta, vda_5050::Pose &endPose, vda_5050::Pose &prev_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(theta))
|
||||
{
|
||||
double xAB = endPose.getX() - prev_Pose.getX();
|
||||
double yAB = endPose.getY() - prev_Pose.getY();
|
||||
double d = sqrt(xAB * xAB + yAB * yAB);
|
||||
double xC = endPose.getX() + d * cos(theta);
|
||||
double yC = endPose.getY() + d * sin(theta);
|
||||
double xBC = xC - endPose.getX();
|
||||
double yBC = yC - endPose.getY();
|
||||
double dAB = sqrt(xAB * xAB + yAB * yAB);
|
||||
double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose &endPose, geometry_msgs::Pose &prev_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(theta))
|
||||
{
|
||||
double xAB = endPose.position.x - prev_Pose.position.x;
|
||||
double yAB = endPose.position.y - prev_Pose.position.y;
|
||||
double d = sqrt(xAB * xAB + yAB * yAB);
|
||||
double xC = endPose.position.x + d * cos(theta);
|
||||
double yC = endPose.position.y + d * sin(theta);
|
||||
double xBC = xC - endPose.position.x;
|
||||
double yBC = yC - endPose.position.y;
|
||||
double dAB = sqrt(xAB * xAB + yAB * yAB);
|
||||
double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
void setYawAllPosesOnEdge(std::vector<vda_5050::Pose> &posesOnEdge, bool reverse)
|
||||
{
|
||||
if (!reverse)
|
||||
{
|
||||
if (!posesOnEdge.empty())
|
||||
{
|
||||
if (posesOnEdge.size() > 2)
|
||||
{
|
||||
for (int i = 0; i < ((int)posesOnEdge.size() - 1); i++)
|
||||
{
|
||||
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
|
||||
posesOnEdge[i + 1].getX(), posesOnEdge[i + 1].getY());
|
||||
posesOnEdge[i].setYaw(theta);
|
||||
}
|
||||
posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size() - 2].getYaw());
|
||||
}
|
||||
else if (posesOnEdge.size() == 2)
|
||||
{
|
||||
if (posesOnEdge[0].getX() != posesOnEdge[1].getX())
|
||||
{
|
||||
double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
|
||||
posesOnEdge[1].getX(), posesOnEdge[1].getY());
|
||||
posesOnEdge[0].setYaw(theta);
|
||||
posesOnEdge[1].setYaw(theta);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!posesOnEdge.empty())
|
||||
{
|
||||
if (posesOnEdge.size() > 2)
|
||||
{
|
||||
for (int i = (int)posesOnEdge.size() - 1; i > 0; i--)
|
||||
{
|
||||
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
|
||||
posesOnEdge[i - 1].getX(), posesOnEdge[i - 1].getY());
|
||||
posesOnEdge[i].setYaw(theta);
|
||||
}
|
||||
posesOnEdge.front().setYaw(posesOnEdge[1].getYaw());
|
||||
}
|
||||
else if (posesOnEdge.size() == 2)
|
||||
{
|
||||
if (posesOnEdge[1].getX() != posesOnEdge[0].getX())
|
||||
{
|
||||
double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
|
||||
posesOnEdge[0].getX(), posesOnEdge[0].getY());
|
||||
posesOnEdge[1].setYaw(theta);
|
||||
posesOnEdge[0].setYaw(theta);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose &Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
|
||||
{
|
||||
double d = 1;
|
||||
double xA = Pose.getX();
|
||||
double yA = Pose.getY();
|
||||
double xB = xA + d * cos(thetaEnd);
|
||||
double yB = yA + d * sin(thetaEnd);
|
||||
double xAB = xB - xA;
|
||||
double yAB = yB - yA;
|
||||
double xC = xA + d * cos(thetaStart);
|
||||
double yC = yA + d * sin(thetaStart);
|
||||
double xAC = xC - xA;
|
||||
double yAC = yC - yA;
|
||||
double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose &startPose, geometry_msgs::Pose &next_Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(theta))
|
||||
{
|
||||
double xAB = next_Pose.position.x - startPose.position.x;
|
||||
double yAB = next_Pose.position.y - startPose.position.y;
|
||||
double d = sqrt(xAB * xAB + yAB * yAB);
|
||||
double xC = startPose.position.x + d * cos(theta);
|
||||
double yC = startPose.position.y + d * sin(theta);
|
||||
double xAC = xC - startPose.position.x;
|
||||
double yAC = yC - startPose.position.y;
|
||||
double dAB = sqrt(xAB * xAB + yAB * yAB);
|
||||
double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("xC: %f, yC: %f", xC, yC);
|
||||
// ROS_WARN("dAB: %f", dAB);
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose &Pose)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
|
||||
{
|
||||
double d = 1;
|
||||
double xA = Pose.position.x;
|
||||
double yA = Pose.position.y;
|
||||
double xB = xA + d * cos(thetaEnd);
|
||||
double yB = yA + d * sin(thetaEnd);
|
||||
double xAB = xB - xA;
|
||||
double yAB = yB - yA;
|
||||
double xC = xA + d * cos(thetaStart);
|
||||
double yC = yA + d * sin(thetaStart);
|
||||
double xAC = xC - xA;
|
||||
double yAC = yC - yA;
|
||||
double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
delta_angle = acos(cos_a);
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
}
|
||||
return delta_angle;
|
||||
}
|
||||
|
||||
double calculateDistance(vda_5050::Pose &pose1, vda_5050::Pose &pose2)
|
||||
{
|
||||
double dx = pose1.getX() - pose2.getX();
|
||||
double dy = pose1.getY() - pose2.getY();
|
||||
return std::sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
double computeDeltaAngle(vda_5050::Pose &Pose1, vda_5050::Pose &Pose2)
|
||||
{
|
||||
double delta_angle = 0;
|
||||
|
||||
double xA = Pose1.getX();
|
||||
double yA = Pose1.getY();
|
||||
double xB = Pose2.getX();
|
||||
double yB = Pose2.getY();
|
||||
double xAB = xB - xA;
|
||||
double yAB = yB - yA;
|
||||
double d = sqrt(xAB * xAB + yAB * yAB);
|
||||
double xC = xB + d * cos(Pose2.getYaw());
|
||||
double yC = yB + d * sin(Pose2.getYaw());
|
||||
double xBC = xC - xB;
|
||||
double yBC = yC - yB;
|
||||
double cos_a = (xAB * xBC + yAB * yBC) / (d * d);
|
||||
if (cos_a > 1)
|
||||
cos_a = 1;
|
||||
else if (cos_a < (-1))
|
||||
cos_a = -1;
|
||||
double delta_angle_tmp = acos(cos_a);
|
||||
if (delta_angle_tmp >= 1.5707963268)
|
||||
{
|
||||
delta_angle = M_PI - delta_angle_tmp;
|
||||
}
|
||||
else
|
||||
{
|
||||
delta_angle = delta_angle_tmp;
|
||||
}
|
||||
// delta_angle = delta_angle*180/M_PI;
|
||||
// ROS_WARN("delta_angle: %f", delta_angle);
|
||||
return delta_angle;
|
||||
}
|
||||
}
|
||||
37
Controllers/Packages/amr_control/src/amr_monitor.cpp
Normal file
37
Controllers/Packages/amr_control/src/amr_monitor.cpp
Normal file
@@ -0,0 +1,37 @@
|
||||
#include "amr_control/amr_monitor.h"
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
AmrMonitor::AmrMonitor() {}
|
||||
|
||||
AmrMonitor::AmrMonitor(const ros::NodeHandle &nh)
|
||||
{
|
||||
if (!initalized_)
|
||||
this->init(nh);
|
||||
}
|
||||
|
||||
AmrMonitor::~AmrMonitor()
|
||||
{
|
||||
if (odom_sub_)
|
||||
odom_sub_.reset();
|
||||
}
|
||||
|
||||
void AmrMonitor::init(const ros::NodeHandle &nh)
|
||||
{
|
||||
if (!initalized_)
|
||||
{
|
||||
ros::NodeHandle nh_core;
|
||||
odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh_core);
|
||||
initalized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool AmrMonitor::getVelocity(nav_2d_msgs::Twist2D &velocity)
|
||||
{
|
||||
if (!odom_sub_)
|
||||
return false;
|
||||
else
|
||||
velocity = odom_sub_->getTwist();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
3491
Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
Normal file
3491
Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
Normal file
File diff suppressed because it is too large
Load Diff
85
Controllers/Packages/amr_control/src/amr_safety.cpp
Normal file
85
Controllers/Packages/amr_control/src/amr_safety.cpp
Normal file
@@ -0,0 +1,85 @@
|
||||
#include "amr_control/amr_safety.h"
|
||||
|
||||
namespace amr_control
|
||||
{
|
||||
AmrSafety::AmrSafety() {}
|
||||
|
||||
AmrSafety::AmrSafety(const ros::NodeHandle &nh)
|
||||
{
|
||||
if (!initalized_)
|
||||
this->init(nh);
|
||||
}
|
||||
|
||||
AmrSafety::~AmrSafety() {}
|
||||
|
||||
void AmrSafety::init(const ros::NodeHandle &nh)
|
||||
{
|
||||
if (!initalized_)
|
||||
{
|
||||
|
||||
initalized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool AmrSafety::getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr)
|
||||
{
|
||||
if (plc_controller_ptr == nullptr)
|
||||
return false;
|
||||
else
|
||||
{
|
||||
plc_controller_ptr_ = plc_controller_ptr;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
void AmrSafety::safetyHandle(
|
||||
nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel)
|
||||
{
|
||||
if (plc_controller_ptr_ == nullptr)
|
||||
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
|
||||
else if (!plc_controller_ptr_->checkConnected())
|
||||
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
|
||||
else
|
||||
{
|
||||
nav_2d_msgs::Twist2D velocity_warn;
|
||||
velocity_warn.x = 0.15;
|
||||
velocity_warn.theta = 0.5;
|
||||
nav_2d_msgs::Twist2D velocity_stop;
|
||||
|
||||
// Dieu khien chuyen vung giam toc va mute
|
||||
bool zone_warn_cases;
|
||||
if (velocity.x >= velocity_warn.x)
|
||||
zone_warn_cases = false;
|
||||
else if (velocity.x < velocity_warn.x && velocity.x > 0.05)
|
||||
zone_warn_cases = true;
|
||||
plc_controller_ptr_->writeM(52, zone_warn_cases);
|
||||
|
||||
// Nhan tin hieu 3 vung tu lidar
|
||||
bool output_lidar[4];
|
||||
plc_controller_ptr_->mulGetM(30, 33, output_lidar);
|
||||
if (output_lidar[0] && output_lidar[1] && output_lidar[2])
|
||||
cmd_vel = velocity_stop;
|
||||
else if (output_lidar[0] && !output_lidar[1] && output_lidar[2])
|
||||
cmd_vel = velocity_warn;
|
||||
else if (output_lidar[0] && !output_lidar[1] && !output_lidar[2])
|
||||
cmd_vel = cmd_vel_max;
|
||||
else
|
||||
cmd_vel = velocity_stop;
|
||||
}
|
||||
}
|
||||
|
||||
void AmrSafety::writeMutesSafety(bool value)
|
||||
{
|
||||
if (plc_controller_ptr_ == nullptr)
|
||||
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
|
||||
else if (!plc_controller_ptr_->checkConnected())
|
||||
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
|
||||
else
|
||||
{
|
||||
bool f_data;
|
||||
plc_controller_ptr_->getM(53, f_data);
|
||||
if (f_data != value)
|
||||
plc_controller_ptr_->writeM(53, value);
|
||||
}
|
||||
}
|
||||
}
|
||||
1260
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
Normal file
1260
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
Normal file
File diff suppressed because it is too large
Load Diff
279
Controllers/Packages/amr_control/test/test_move_base.cpp
Normal file
279
Controllers/Packages/amr_control/test/test_move_base.cpp
Normal file
@@ -0,0 +1,279 @@
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <move_base_core/navigation.h>
|
||||
#include <amr_control/moveTo.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <std_msgs/Int16.h>
|
||||
#include <geometry_msgs/Vector3.h>
|
||||
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_utils/path_ops.h>
|
||||
#include <vda5050_msgs/Order.h>
|
||||
#include <vda5050_msgs/InstantActions.h>
|
||||
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
|
||||
move_base_core::BaseNavigation::Ptr move_base = nullptr;
|
||||
geometry_msgs::PoseStamped amcl_pose;
|
||||
boost::mutex ctr_mutex;
|
||||
bool pub_order = false;
|
||||
amr_control::moveTo order;
|
||||
bool pub_cancel = false;
|
||||
|
||||
void moveToCallback(const amr_control::moveTo::ConstPtr& msg)
|
||||
{
|
||||
// boost::unique_lock<boost::mutex> lock(ctr_mutex);
|
||||
if(move_base)
|
||||
{
|
||||
try
|
||||
{
|
||||
geometry_msgs::PoseStamped goal = nav_2d_utils::pose2DToPoseStamped(msg->goal, "map", ros::Time::now());
|
||||
geometry_msgs::PoseStamped of_goal = move_base_core::offset_goal(amcl_pose, msg->goal.x);
|
||||
double xy_goal_tolerance = 0.04;
|
||||
double yaw_goal_tolerance = 0.06;
|
||||
// ROS_INFO_STREAM(of_goal);
|
||||
switch ((int)msg->maker.data)
|
||||
{
|
||||
case 1:
|
||||
move_base->moveTo(goal, move_base_core::PICK_UP, xy_goal_tolerance, yaw_goal_tolerance);
|
||||
break;
|
||||
case 2:
|
||||
move_base->moveTo(goal, move_base_core::POP_DOWN, xy_goal_tolerance, yaw_goal_tolerance);
|
||||
break;
|
||||
case 3:
|
||||
move_base->moveTo(goal, move_base_core::POSITION, xy_goal_tolerance, yaw_goal_tolerance);
|
||||
break;
|
||||
case 4:
|
||||
move_base->moveTo(goal, move_base_core::CHARGER, xy_goal_tolerance, yaw_goal_tolerance);
|
||||
break;
|
||||
case 5:
|
||||
move_base->moveStraightTo(of_goal, xy_goal_tolerance);
|
||||
break;
|
||||
case 6:
|
||||
move_base->rotateTo(goal, yaw_goal_tolerance);
|
||||
break;
|
||||
default:
|
||||
throw std::bad_function_call();
|
||||
break;
|
||||
}
|
||||
|
||||
geometry_msgs::Vector3 linear;
|
||||
linear.x = 1.1;
|
||||
move_base->setTwistLinear(linear);
|
||||
linear.x = -0.5;
|
||||
move_base->setTwistLinear(linear);
|
||||
}
|
||||
catch(const std::exception& e)
|
||||
{
|
||||
ROS_ERROR_STREAM(e.what() << "\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void getposeCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
|
||||
{
|
||||
amcl_pose.header = msg->header;
|
||||
amcl_pose.pose.position = msg->pose.pose.position;
|
||||
amcl_pose.pose.orientation = msg->pose.pose.orientation;
|
||||
}
|
||||
|
||||
void setTiwstCallback(const geometry_msgs::Vector3::ConstPtr& msg)
|
||||
{
|
||||
if(move_base)
|
||||
{
|
||||
move_base->setTwistLinear(*msg);
|
||||
}
|
||||
}
|
||||
|
||||
void cancelCallback(const std_msgs::Bool::ConstPtr& msg)
|
||||
{
|
||||
move_base->cancel();
|
||||
}
|
||||
|
||||
void pauseCallback(const std_msgs::Bool::ConstPtr& msg)
|
||||
{
|
||||
move_base->pause();
|
||||
}
|
||||
|
||||
void resumeCallback(const std_msgs::Bool::ConstPtr& msg)
|
||||
{
|
||||
move_base->resume();
|
||||
}
|
||||
|
||||
void order_msg_handle(const vda5050_msgs::Order::ConstPtr& msg)
|
||||
{
|
||||
if(msg->serialNumber == "AMR-150")
|
||||
{
|
||||
ros::Duration(0.5).sleep();
|
||||
if(!msg->nodes.empty())
|
||||
{
|
||||
order.header.frame_id = "map";
|
||||
order.header.stamp = ros::Time::now();
|
||||
order.goal.x = msg->nodes.back().nodePosition.x;
|
||||
order.goal.y = msg->nodes.back().nodePosition.y;
|
||||
order.goal.theta = msg->nodes.back().nodePosition.theta;
|
||||
if(!msg->nodes.back().actions.empty())
|
||||
{
|
||||
if(msg->nodes.back().actions.front().actionType == "startInPallet")
|
||||
order.maker.data = 1;
|
||||
else
|
||||
order.maker.data = 3;
|
||||
}
|
||||
else
|
||||
order.maker.data = 3;
|
||||
pub_order = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void instantActions_msg_handle(const vda5050_msgs::InstantActions::ConstPtr& msg)
|
||||
{
|
||||
if(msg->serialNumber == "AMR-150")
|
||||
{
|
||||
for(int i=0; i< msg->actions.size(); i++)
|
||||
{
|
||||
if(msg->actions[i].actionType == "cancelOrder") pub_cancel = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool getRobotPose(geometry_msgs::PoseStamped& global_pose, std::shared_ptr<tf2_ros::Buffer> tf_)
|
||||
{
|
||||
std::string global_frame_ = "map";
|
||||
std::string robot_base_frame_ = "base_link";
|
||||
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROS::getRobotPose");
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
|
||||
geometry_msgs::PoseStamped robot_pose;
|
||||
tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
|
||||
robot_pose.header.frame_id = robot_base_frame_;
|
||||
robot_pose.header.stamp = ros::Time();
|
||||
ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
|
||||
|
||||
// get the global pose of the robot
|
||||
try
|
||||
{
|
||||
// use current time if possible (makes sure it's not in the future)
|
||||
if (tf_->canTransform(global_frame_, robot_base_frame_, current_time))
|
||||
{
|
||||
geometry_msgs::TransformStamped transform = tf_->lookupTransform(global_frame_, robot_base_frame_, current_time);
|
||||
tf2::doTransform(robot_pose, global_pose, transform);
|
||||
}
|
||||
// use the latest otherwise
|
||||
else
|
||||
{
|
||||
tf_->transform(robot_pose, global_pose, global_frame_);
|
||||
}
|
||||
}
|
||||
catch (tf2::LookupException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ConnectivityException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
catch (tf2::ExtrapolationException& ex)
|
||||
{
|
||||
ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
|
||||
return false;
|
||||
}
|
||||
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
|
||||
// check global_pose timeout
|
||||
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > 1.0)
|
||||
{
|
||||
ROS_WARN_THROTTLE(1.0,
|
||||
// ROS_WARN(
|
||||
"Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
|
||||
current_time.toSec(), global_pose.header.stamp.toSec(), 1.0);
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv){
|
||||
ros::init(argc, argv, "move_base_node");
|
||||
ros::NodeHandle nh = ros::NodeHandle("move_base_node");
|
||||
ros::NodeHandle nh_;
|
||||
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
|
||||
tf2_ros::TransformListener tf2(*buffer);
|
||||
|
||||
pluginlib::ClassLoader<move_base_core::BaseNavigation> mb_loader("move_base_core", "move_base_core::BaseNavigation");
|
||||
|
||||
std::string obj_name("move_base::MoveBase");
|
||||
try
|
||||
{
|
||||
move_base = mb_loader.createUniqueInstance(obj_name);
|
||||
ROS_INFO("Created object %s susseced", obj_name.c_str());
|
||||
move_base->initialize(buffer);
|
||||
}
|
||||
catch (pluginlib::PluginlibException &ex)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
|
||||
return 1;
|
||||
}
|
||||
catch(std::exception &e)
|
||||
{
|
||||
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
|
||||
return 1;
|
||||
}
|
||||
ros::Subscriber moveto_sub = nh.subscribe("moveTo", 1000, moveToCallback);
|
||||
ros::Subscriber set_twist_sub = nh.subscribe("setTiwst", 1000, setTiwstCallback);
|
||||
ros::Subscriber cancel_sub = nh.subscribe("cancel", 1000, cancelCallback);
|
||||
ros::Subscriber pause_sub = nh.subscribe("pause", 1000, pauseCallback);
|
||||
ros::Subscriber resume_sub = nh.subscribe("resume", 1000, resumeCallback);
|
||||
ros::Subscriber amcl_pose_sub = nh.subscribe("amcl_pose", 1000, getposeCallback);
|
||||
ros::Subscriber order_msg_sub_ = nh_.subscribe("/order",1000, order_msg_handle);
|
||||
ros::Subscriber instantAc_msg_sub_ = nh_.subscribe("/instantActions",1000, instantActions_msg_handle);
|
||||
ros::Publisher moveto_pub = nh.advertise<amr_control::moveTo>("moveTo", 1000);
|
||||
ros::Publisher cancel_pub = nh.advertise<std_msgs::Bool>("cancel", 1000);
|
||||
ros::Rate r(10);
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
if(pub_order)
|
||||
{
|
||||
moveto_pub.publish(order);
|
||||
pub_order = false;
|
||||
}
|
||||
else if (pub_cancel)
|
||||
{
|
||||
std_msgs::Bool value;
|
||||
value.data = true;
|
||||
cancel_pub.publish(value);
|
||||
pub_cancel = false;
|
||||
}
|
||||
getRobotPose(amcl_pose, buffer);
|
||||
// boost::unique_lock<boost::mutex> lock(ctr_mutex);
|
||||
// {
|
||||
if(move_base && move_base->nav_feedback_)
|
||||
{
|
||||
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::ABORTED)
|
||||
// ROS_ERROR("ABORTED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
||||
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::PAUSED)
|
||||
// ROS_WARN("PAUSED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
||||
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::PREEMPTED)
|
||||
// ROS_WARN("PREEMPTED %s", move_base->nav_feedback_->feed_back_str.c_str());
|
||||
// if(move_base->nav_feedback_->navigation_state == move_base_core::State::SUCCEEDED)
|
||||
// ROS_INFO_STREAM("SUCCEEDED \n" << move_base->nav_feedback_->current_pose);
|
||||
}
|
||||
// }
|
||||
ros::spinOnce();
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
ros::spin();
|
||||
return(0);
|
||||
}
|
||||
15
Controllers/Packages/amr_control/test/vda_5050_api.cpp
Normal file
15
Controllers/Packages/amr_control/test/vda_5050_api.cpp
Normal file
@@ -0,0 +1,15 @@
|
||||
#include <ros/ros.h>
|
||||
#include "amr_control/amr_vda_5050_client_api.h"
|
||||
|
||||
std::shared_ptr<amr_control::VDA5050ClientAPI> client;
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Initialize the ROS node
|
||||
ros::init(argc, argv, "test_imr_nova_control");
|
||||
ros::start();
|
||||
|
||||
client = std::make_shared<amr_control::VDA5050ClientAPI>();
|
||||
ros::spin();
|
||||
client.reset();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user