git commit -m "first commit for v2"
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user