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

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