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

update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 15:46:20 +07:00
parent 7b27905ad4
commit dc652575d1
69 changed files with 3834 additions and 22719 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS
delta_modbus
nova5_control
loc_core
move_base_core
amr_comunication
vda5050_msgs
)
@@ -138,20 +137,27 @@ add_library(${PROJECT_NAME}
src/amr_monitor.cpp
src/amr_safety.cpp
src/amr_opc_ua_server_api.cpp
src/amr_vda_5050_client_api.cpp
src/amr_make_plan_with_order.cpp
# src/amr_vda_5050_client_api.cpp
# src/amr_make_plan_with_order.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
# Libraries will be found via LD_LIBRARY_PATH or system paths
set_target_properties(${PROJECT_NAME} PROPERTIES
SKIP_BUILD_RPATH TRUE
BUILD_WITH_INSTALL_RPATH FALSE
INSTALL_RPATH_USE_LINK_PATH FALSE
)
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp)
add_executable(vda_5050_api_test test/vda_5050_api.cpp)
# add_executable(${PROJECT_NAME}_node src/amr_control_node.cpp)
# add_executable(vda_5050_api_test test/vda_5050_api.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -161,17 +167,31 @@ add_executable(vda_5050_api_test test/vda_5050_api.cpp)
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node
${PROJECT_NAME}
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# add_dependencies(${PROJECT_NAME}_node
# ${PROJECT_NAME}
# ${${PROJECT_NAME}_EXPORTED_TARGETS}
# ${catkin_EXPORTED_TARGETS}
# )
# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
# Libraries will be found via LD_LIBRARY_PATH or system paths
# set_target_properties(${PROJECT_NAME}_node PROPERTIES
# SKIP_BUILD_RPATH TRUE
# BUILD_WITH_INSTALL_RPATH FALSE
# INSTALL_RPATH_USE_LINK_PATH FALSE
# )
add_dependencies(vda_5050_api_test
${PROJECT_NAME}
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
# add_dependencies(vda_5050_api_test
# ${PROJECT_NAME}
# ${${PROJECT_NAME}_EXPORTED_TARGETS}
# ${catkin_EXPORTED_TARGETS}
# )
# Fix circular RPATH dependency: disable automatic RPATH detection to avoid cycle warnings
# Libraries will be found via LD_LIBRARY_PATH or system paths
# set_target_properties(vda_5050_api_test PROPERTIES
# SKIP_BUILD_RPATH TRUE
# BUILD_WITH_INSTALL_RPATH FALSE
# INSTALL_RPATH_USE_LINK_PATH FALSE
# )
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}
@@ -180,17 +200,17 @@ target_link_libraries(${PROJECT_NAME}
${Boost_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_node
${PROJECT_NAME}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
# target_link_libraries(${PROJECT_NAME}_node
# ${PROJECT_NAME}
# ${catkin_LIBRARIES}
# ${Boost_LIBRARIES}
# )
target_link_libraries(vda_5050_api_test
${PROJECT_NAME}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
# target_link_libraries(vda_5050_api_test
# ${PROJECT_NAME}
# ${catkin_LIBRARIES}
# ${Boost_LIBRARIES}
# )
#############
@@ -209,9 +229,9 @@ target_link_libraries(vda_5050_api_test
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
# install(TARGETS ${PROJECT_NAME}_node vda_5050_api_test
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html

23
Controllers/Packages/amr_control/include/amr_control/amr_control.h
View File

@@ -18,11 +18,17 @@
#include "amr_control/amr_vda_5050_client_api.h"
// Objects
#include "move_base_core/navigation.h"
#include "loc_core/localization.h"
#include "amr_control/amr_monitor.h"
#include "nova5_control/imr_nova_control.h"
#include "amr_control/amr_safety.h"
#include <move_base_core/common.h> // For TFListenerPtr
#include <move_base_core/navigation.h>
#include <loc_core/localization.h>
#include <amr_control/amr_monitor.h>
#include <nova5_control/imr_nova_control.h>
#include <amr_control/amr_safety.h>
#include <robot/console.h>
// Plugin Loader
#include <robot/plugin_loader_helper.h>
#include <boost/dll/import.hpp>
namespace amr_control
{
@@ -68,6 +74,8 @@ namespace amr_control
bool initalized_;
ros::NodeHandle nh_;
std::shared_ptr<tf2_ros::Buffer> tf_;
// robot::TFListenerPtr tf_core_ptr_;
ros::Subscriber is_detected_maker_sub_;
std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;
@@ -77,8 +85,9 @@ namespace amr_control
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_;
move_base_core::BaseNavigation::Ptr move_base_ptr_;
boost::function<move_base_core::BaseNavigation::Ptr()> move_base_loader_;
// Synchronous processing thread
std::shared_ptr<std::thread> move_base_thr_;
std::mutex init_move_base_mutex_;

10
Controllers/Packages/amr_control/include/amr_control/amr_opc_ua_server_api.h
View File

@@ -2,11 +2,11 @@
#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 <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

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

@@ -50,7 +50,6 @@
<!-- <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>
@@ -63,7 +62,6 @@
<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>

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

@@ -13,7 +13,6 @@ namespace amr_control
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
this->init(nh, buffer);
@@ -29,7 +28,6 @@ namespace amr_control
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
}
@@ -221,358 +219,366 @@ namespace amr_control
throw std::runtime_error("tf2_ros::Buffer object is null");
try
{
move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name);
ROS_INFO("Created object %s susseced", obj_name.c_str());
move_base_ptr_->initialize(tf_);
ros::Rate r(3);
do
{
r.sleep();
ros::spinOnce();
} while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
// robot::log_info("[%s:%d]\n Loading MoveBase library...", __FILE__, __LINE__);
// robot::PluginLoaderHelper loader;
// std::string path_file_so = loader.findLibraryPath("MoveBase");
// move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
// path_file_so,
// "MoveBase", boost::dll::load_mode::append_decorations);
// robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
// move_base_ptr_ = create_plugin();
// move_base_ptr_->initialize(tf_core_ptr_);
if (move_base_ptr_ != nullptr &&
move_base_ptr_->nav_feedback_ != nullptr &&
move_base_ptr_->nav_feedback_->is_ready)
{
geometry_msgs::Vector3 linear;
linear.x = 0.3;
move_base_ptr_->setTwistLinear(linear);
linear.x = -0.3;
move_base_ptr_->setTwistLinear(linear);
}
// ros::Rate r(3);
// do
// {
// r.sleep();
// ros::spinOnce();
// } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
// if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready)
// {
// geometry_msgs::Vector3 linear;
// linear.x = 0.3;
// move_base_ptr_->setTwistLinear(linear);
// linear.x = -0.3;
// move_base_ptr_->setTwistLinear(linear);
// }
}
catch (pluginlib::PluginlibException &ex)
catch (const std::exception &e)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
// robot::log_error("[%s:%d]\n Exception: %s", __FILE__, __LINE__, e.what());
exit(1);
}
catch (std::exception &e)
catch (...)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
// robot::log_error("[%s:%d]\n Unknown exception occurred", __FILE__, __LINE__);
exit(1);
}
}
void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
{
this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
}
// 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::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_);
// 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();
}
// 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;
}
// 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::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();
// 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_->checkConnected())
// {
// state = amr_control::State::FAILED;
// return;
// }
if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
{
int shipping_regsister = 119, receiving_regsister = 116;
plc_controller_ptr_->setM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
ros::Duration(0.1).sleep();
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
// if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// {
// int shipping_regsister = 119, receiving_regsister = 116;
// plc_controller_ptr_->setM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister);
// ros::Duration(0.1).sleep();
// plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister);
ros::Rate r(5);
while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
{
state = amr_control::State::RUNNING;
bool output_belt[2];
plc_controller_ptr_->mulGetM(124, 125, output_belt);
// ros::Rate r(5);
// while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
// {
// state = amr_control::State::RUNNING;
// bool output_belt[2];
// plc_controller_ptr_->mulGetM(124, 125, output_belt);
bool enable_shipping = output_belt[0];
bool enable_receiving = output_belt[1];
if (!enable_shipping && !enable_receiving)
{
state = amr_control::State::FINISHED;
break;
}
r.sleep();
}
// bool enable_shipping = output_belt[0];
// bool enable_receiving = output_belt[1];
// if (!enable_shipping && !enable_receiving)
// {
// state = amr_control::State::FINISHED;
// break;
// }
// r.sleep();
// }
if (cancel_ || !plc_controller_ptr_->checkConnected())
state = amr_control::State::FAILED;
// if (cancel_ || !plc_controller_ptr_->checkConnected())
// state = amr_control::State::FAILED;
plc_controller_ptr_->close();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
// plc_controller_ptr_->close();
// }
// if (plc_controller_ptr_)
// plc_controller_ptr_.reset();
std::lock_guard<std::mutex> lock(this->arm_mutex_);
this->belt_joined_ = false;
}
// 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::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();
// 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_->checkConnected())
// {
// state = amr_control::State::FAILED;
// return;
// }
if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
{
int shipping_regsister = 119, receiving_regsister = 116;
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->setM(receiving_regsister);
ros::Duration(0.1).sleep();
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
// if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// {
// int shipping_regsister = 119, receiving_regsister = 116;
// plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->setM(receiving_regsister);
// ros::Duration(0.1).sleep();
// plc_controller_ptr_->resetM(shipping_regsister);
// plc_controller_ptr_->resetM(receiving_regsister);
ros::Rate r(5);
while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
{
state = amr_control::State::RUNNING;
bool output_belt[2];
plc_controller_ptr_->mulGetM(124, 125, output_belt);
// ros::Rate r(5);
// while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
// {
// state = amr_control::State::RUNNING;
// bool output_belt[2];
// plc_controller_ptr_->mulGetM(124, 125, output_belt);
bool enable_shipping = output_belt[0];
bool enable_receiving = output_belt[1];
if (!enable_shipping && !enable_receiving)
{
state = amr_control::State::FINISHED;
break;
}
r.sleep();
}
if (this->cancel_ || !plc_controller_ptr_->checkConnected())
state = amr_control::State::FAILED;
// bool enable_shipping = output_belt[0];
// bool enable_receiving = output_belt[1];
// if (!enable_shipping && !enable_receiving)
// {
// state = amr_control::State::FINISHED;
// break;
// }
// r.sleep();
// }
// if (this->cancel_ || !plc_controller_ptr_->checkConnected())
// state = amr_control::State::FAILED;
plc_controller_ptr_->close();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
// plc_controller_ptr_->close();
// }
// if (plc_controller_ptr_)
// plc_controller_ptr_.reset();
std::lock_guard<std::mutex> lock(this->arm_mutex_);
this->belt_joined_ = false;
}
// 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();
// 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;
// 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_);
// 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();
}
// while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
// {
// if (!this->monitor_ptr_)
// continue;
// nav_2d_msgs::Twist2D velocity;
// if (this->monitor_ptr_->getVelocity(velocity))
// {
// cmd_vel_mtx.lock();
// this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
// cmd_vel_mtx.unlock();
// }
if (move_base_ptr_ != nullptr &&
move_base_ptr_->nav_feedback_ != nullptr &&
move_base_ptr_->nav_feedback_->is_ready)
{
if (velocity.x <= -0.01)
this->amr_safety_ptr_->writeMutesSafety(true);
else
{
this->amr_safety_ptr_->writeMutesSafety(this->muted_);
}
}
// if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready)
// {
// if (velocity.x <= -0.01)
// this->amr_safety_ptr_->writeMutesSafety(true);
// else
// {
// this->amr_safety_ptr_->writeMutesSafety(this->muted_);
// }
// }
bool have_goods;
int have_goods_regsister = 142;
if (!OpcUAServerAPI::belt_cancel_)
{
plc_controller_ptr_->getM(have_goods_regsister, have_goods);
OpcUAServerAPI::have_goods_ = have_goods;
}
else
{
plc_controller_ptr_->resetM(have_goods_regsister);
}
// bool have_goods;
// int have_goods_regsister = 142;
// if (!OpcUAServerAPI::belt_cancel_)
// {
// plc_controller_ptr_->getM(have_goods_regsister, have_goods);
// OpcUAServerAPI::have_goods_ = have_goods;
// }
// else
// {
// plc_controller_ptr_->resetM(have_goods_regsister);
// }
if (vda_5050_client_api_ptr_)
{
amr_control::OperatingMode mode_;
bool operating_mode[4];
plc_controller_ptr_->mulGetM(14, 17, operating_mode);
if (operating_mode[0])
mode_ = amr_control::OperatingMode::AUTOMATIC;
else if (operating_mode[2])
mode_ = amr_control::OperatingMode::SERVICE;
else if (operating_mode[3])
mode_ = amr_control::OperatingMode::MANUAL;
// if (vda_5050_client_api_ptr_)
// {
// amr_control::OperatingMode mode_;
// bool operating_mode[4];
// plc_controller_ptr_->mulGetM(14, 17, operating_mode);
// if (operating_mode[0])
// mode_ = amr_control::OperatingMode::AUTOMATIC;
// else if (operating_mode[2])
// mode_ = amr_control::OperatingMode::SERVICE;
// else if (operating_mode[3])
// mode_ = amr_control::OperatingMode::MANUAL;
switch (mode_)
{
case amr_control::OperatingMode::AUTOMATIC:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
break;
case amr_control::OperatingMode::MANUAL:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
break;
case amr_control::OperatingMode::SERVICE:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
break;
default:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
break;
}
}
r.sleep();
ros::spinOnce();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
if (amr_safety_ptr_)
amr_safety_ptr_.reset();
}
}
// switch (mode_)
// {
// case amr_control::OperatingMode::AUTOMATIC:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
// break;
// case amr_control::OperatingMode::MANUAL:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
// break;
// case amr_control::OperatingMode::SERVICE:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
// break;
// default:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
// break;
// }
// }
// r.sleep();
// ros::spinOnce();
// }
// if (plc_controller_ptr_)
// plc_controller_ptr_.reset();
// if (amr_safety_ptr_)
// amr_safety_ptr_.reset();
// }
// }
void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
{
this->muted_ = msg->data;
}
// void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
// {
// this->muted_ = msg->data;
// }
void AmrController::threadHandle()
{
ros::Rate r(5);
while (ros::ok())
{
if (this->arm_thread_.joinable())
{
std::lock_guard<std::mutex> lock(this->arm_mutex_);
{
if (!this->arm_joined_)
{
this->arm_thread_.join();
}
}
}
// void AmrController::threadHandle()
// {
// ros::Rate r(5);
// while (ros::ok())
// {
// if (this->arm_thread_.joinable())
// {
// std::lock_guard<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 (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);
// if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready)
// {
// nav_2d_msgs::Twist2D velocity;
// if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
// {
// this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
// this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
geometry_msgs::Vector3 linear;
geometry_msgs::Vector3 angular;
// geometry_msgs::Vector3 linear;
// geometry_msgs::Vector3 angular;
cmd_vel_mtx.lock();
linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
angular.z = this->cmd_vel_max_.theta;
cmd_vel_mtx.unlock();
// cmd_vel_mtx.lock();
// linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
// angular.z = this->cmd_vel_max_.theta;
// cmd_vel_mtx.unlock();
this->move_base_ptr_->setTwistLinear(linear);
linear.x *= -1.0;
this->move_base_ptr_->setTwistLinear(linear);
this->move_base_ptr_->setTwistAngular(angular);
}
}
r.sleep();
ros::spinOnce();
}
}
// this->move_base_ptr_->setTwistLinear(linear);
// linear.x *= -1.0;
// this->move_base_ptr_->setTwistLinear(linear);
// this->move_base_ptr_->setTwistAngular(angular);
// }
// }
// r.sleep();
// ros::spinOnce();
// }
// }
}

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

File diff suppressed because it is too large Load Diff