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

update ros

Browse Source
This commit is contained in:
HiepLM
2026-01-07 09:28:55 +07:00
parent 7cea6bb120
commit ec6d5746e3
58 changed files with 22752 additions and 3773 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -233,23 +233,23 @@ namespace amr_control
move_base_ptr_ = move_base_loader_();
// move_base_ptr_->initialize(tf_core_ptr_);
// ros::Rate r(3);
// do
// {
// r.sleep();
// ros::spinOnce();
// } while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
ros::Rate r(3);
do
{
r.sleep();
ros::spinOnce();
} while (ros::ok() && !move_base_ptr_->getFeedback()->is_ready);
// if (move_base_ptr_ != nullptr &&
// move_base_ptr_->nav_feedback_ != nullptr &&
// move_base_ptr_->nav_feedback_->is_ready)
// {
// geometry_msgs::Vector3 linear;
// linear.x = 0.3;
// move_base_ptr_->setTwistLinear(linear);
// linear.x = -0.3;
// move_base_ptr_->setTwistLinear(linear);
// }
if (move_base_ptr_ != nullptr &&
move_base_ptr_->getFeedback() != nullptr &&
move_base_ptr_->getFeedback()->is_ready)
{
robot_geometry_msgs::Vector3 linear;
linear.x = 0.3;
move_base_ptr_->setTwistLinear(linear);
linear.x = -0.3;
move_base_ptr_->setTwistLinear(linear);
}
}
catch (const std::exception &e)
{
@@ -263,325 +263,325 @@ namespace amr_control
}
}
// void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
// {
// this->monitor_ptr_ = std::make_shared<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_->getFeedback() != nullptr &&
move_base_ptr_->getFeedback()->is_ready)
{
if (velocity.x <= -0.01)
this->amr_safety_ptr_->writeMutesSafety(true);
else
{
this->amr_safety_ptr_->writeMutesSafety(this->muted_);
}
}
// bool have_goods;
// int have_goods_regsister = 142;
// if (!OpcUAServerAPI::belt_cancel_)
// {
// plc_controller_ptr_->getM(have_goods_regsister, have_goods);
// OpcUAServerAPI::have_goods_ = have_goods;
// }
// else
// {
// plc_controller_ptr_->resetM(have_goods_regsister);
// }
bool have_goods;
int have_goods_regsister = 142;
if (!OpcUAServerAPI::belt_cancel_)
{
plc_controller_ptr_->getM(have_goods_regsister, have_goods);
OpcUAServerAPI::have_goods_ = have_goods;
}
else
{
plc_controller_ptr_->resetM(have_goods_regsister);
}
// if (vda_5050_client_api_ptr_)
// {
// amr_control::OperatingMode mode_;
// bool operating_mode[4];
// plc_controller_ptr_->mulGetM(14, 17, operating_mode);
// if (operating_mode[0])
// mode_ = amr_control::OperatingMode::AUTOMATIC;
// else if (operating_mode[2])
// mode_ = amr_control::OperatingMode::SERVICE;
// else if (operating_mode[3])
// mode_ = amr_control::OperatingMode::MANUAL;
if (vda_5050_client_api_ptr_)
{
amr_control::OperatingMode mode_;
bool operating_mode[4];
plc_controller_ptr_->mulGetM(14, 17, operating_mode);
if (operating_mode[0])
mode_ = amr_control::OperatingMode::AUTOMATIC;
else if (operating_mode[2])
mode_ = amr_control::OperatingMode::SERVICE;
else if (operating_mode[3])
mode_ = amr_control::OperatingMode::MANUAL;
// switch (mode_)
// {
// case amr_control::OperatingMode::AUTOMATIC:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
// break;
// case amr_control::OperatingMode::MANUAL:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
// break;
// case amr_control::OperatingMode::SERVICE:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
// break;
// default:
// vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
// break;
// }
// }
// r.sleep();
// ros::spinOnce();
// }
// if (plc_controller_ptr_)
// plc_controller_ptr_.reset();
// if (amr_safety_ptr_)
// amr_safety_ptr_.reset();
// }
// }
switch (mode_)
{
case amr_control::OperatingMode::AUTOMATIC:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
break;
case amr_control::OperatingMode::MANUAL:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
break;
case amr_control::OperatingMode::SERVICE:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
break;
default:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
break;
}
}
r.sleep();
ros::spinOnce();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
if (amr_safety_ptr_)
amr_safety_ptr_.reset();
}
}
// void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
// {
// this->muted_ = msg->data;
// }
void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
{
this->muted_ = msg->data;
}
// void AmrController::threadHandle()
// {
// ros::Rate r(5);
// while (ros::ok())
// {
// if (this->arm_thread_.joinable())
// {
// std::lock_guard<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_->getFeedback() != nullptr &&
move_base_ptr_->getFeedback()->is_ready)
{
nav_2d_msgs::Twist2D velocity;
if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
{
this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
// geometry_msgs::Vector3 linear;
// geometry_msgs::Vector3 angular;
robot_geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 angular;
// cmd_vel_mtx.lock();
// linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
// angular.z = this->cmd_vel_max_.theta;
// cmd_vel_mtx.unlock();
cmd_vel_mtx.lock();
linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
angular.z = this->cmd_vel_max_.theta;
cmd_vel_mtx.unlock();
// this->move_base_ptr_->setTwistLinear(linear);
// linear.x *= -1.0;
// this->move_base_ptr_->setTwistLinear(linear);
// this->move_base_ptr_->setTwistAngular(angular);
// }
// }
// r.sleep();
// ros::spinOnce();
// }
// }
this->move_base_ptr_->setTwistLinear(linear);
linear.x *= -1.0;
this->move_base_ptr_->setTwistLinear(linear);
this->move_base_ptr_->setTwistAngular(angular);
}
}
r.sleep();
ros::spinOnce();
}
}
}

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

File diff suppressed because it is too large Load Diff

145
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
View File

@@ -20,7 +20,7 @@ namespace amr_control
this->count_ok_max_ = new unsigned int(1);
}
VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<move_base_core::BaseNavigation> move_base,
VDA5050ClientAPI::VDA5050ClientAPI(ros::NodeHandle nh, std::shared_ptr<robot::move_base_core::BaseNavigation> move_base,
std::shared_ptr<loc_core::BaseLocalization> loc_base, std::shared_ptr<AmrMonitor> monitor)
{
VDA5050ClientAPI::move_base_ptr_ = move_base;
@@ -116,7 +116,7 @@ namespace amr_control
void VDA5050ClientAPI::moveTo(vda_5050::Order order, uint8_t &status, std::string &message)
{
geometry_msgs::PoseStamped goal;
robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_)
{
if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@@ -150,9 +150,9 @@ namespace amr_control
p_2d.theta = p.getYaw();
poses.push_back(p_2d);
}
const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
goal = path.poses.back();
VDA5050ClientAPI::plan_pub_.publish(path);
// const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
// goal = path.poses.back();
// VDA5050ClientAPI::plan_pub_.publish(path);
}
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
{
@@ -222,13 +222,13 @@ namespace amr_control
VDA5050ClientAPI::plan_pub_.publish(order_msg);
vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now();
goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map";
goal.pose.position.x = position.x;
goal.pose.position.y = position.y;
tf::Quaternion quat;
quat.setRPY(0.0, 0.0, position.theta);
tf::quaternionTFToMsg(quat, goal.pose.orientation);
// tf3::Transform transform;
// transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
// goal = tf3::toMsg(transform);
}
else
return;
@@ -239,13 +239,13 @@ namespace amr_control
cmd_vel_max_.theta = 0.5;
cmd_vel_max_saved_ = cmd_vel_max_;
geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 linear;
linear.x = cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
geometry_msgs::Vector3 angular;
robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
@@ -254,7 +254,7 @@ namespace amr_control
void VDA5050ClientAPI::moveToDock(vda_5050::Order order, uint8_t &status, std::string &message)
{
geometry_msgs::PoseStamped goal;
robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_)
{
if (global_plan_msg_type_ == std::string("nav_msgs::Path"))
@@ -288,9 +288,9 @@ namespace amr_control
p_2d.theta = p.getYaw();
poses.push_back(p_2d);
}
const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
goal = path.poses.back();
VDA5050ClientAPI::plan_pub_.publish(path);
// const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
// goal = path.poses.back();
// VDA5050ClientAPI::plan_pub_.publish(path);
}
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
{
@@ -359,13 +359,13 @@ namespace amr_control
VDA5050ClientAPI::plan_pub_.publish(order_msg);
vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now();
goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map";
goal.pose.position.x = position.x;
goal.pose.position.y = position.y;
tf::Quaternion quat;
quat.setRPY(0.0, 0.0, position.theta);
tf::quaternionTFToMsg(quat, goal.pose.orientation);
// tf3::Transform transform;
// transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
// goal = tf3::toMsg(transform);
}
else
return;
@@ -376,13 +376,13 @@ namespace amr_control
cmd_vel_max_.theta = 0.5;
cmd_vel_max_saved_ = cmd_vel_max_;
geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 linear;
linear.x = cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -cmd_vel_max_.x;
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
geometry_msgs::Vector3 angular;
robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
@@ -392,19 +392,18 @@ namespace amr_control
void VDA5050ClientAPI::rotateTo(vda_5050::Order order, uint8_t &status, std::string &message)
{
geometry_msgs::PoseStamped goal;
robot_geometry_msgs::PoseStamped goal;
if (VDA5050ClientAPI::move_base_ptr_)
{
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(goal))
{
vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = ros::Time::now();
goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map";
tf::Quaternion quat;
quat.setRPY(0.0, 0.0, position.theta);
tf::quaternionTFToMsg(quat, goal.pose.orientation);
VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
// tf3::Transform transform;
// transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
// goal = tf3::toMsg(transform);
// VDA5050ClientAPI::move_base_ptr_->rotateTo(goal);
}
else
return;
@@ -521,10 +520,10 @@ namespace amr_control
void VDA5050ClientAPI::updateVelocity(double velocity)
{
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING)
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == robot::move_base_core::State::CONTROLLING)
{
if (fabs(velocity) > 2.0)
velocity = (fabs(velocity) / velocity) * 2.0;
@@ -541,10 +540,10 @@ namespace amr_control
void VDA5050ClientAPI::updateAngular(double angular)
{
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING)
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == robot::move_base_core::State::CONTROLLING)
{
if (fabs(angular) > 1.0)
angular = (fabs(angular) / angular) * 1.0;
@@ -645,7 +644,7 @@ namespace amr_control
// Can runing actions be interrupted ?
if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr)
VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
VDA5050ClientAPI::move_base_ptr_->cancel();
*this->cancel_action_ = true;
@@ -653,7 +652,7 @@ namespace amr_control
*this->pause_action_ = false;
ros::Rate r(5);
while (ros::ok() &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state == move_base_core::State::CONTROLLING)
VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state == robot::move_base_core::State::CONTROLLING)
{
ROS_INFO_THROTTLE(1.0, "Waiting to cancel");
r.sleep();
@@ -714,18 +713,18 @@ namespace amr_control
void VDA5050ClientAPI::unDockFromStation(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
while (ros::ok() && !*this->cancel_action_ && nav_state == move_base_core::State::CONTROLLING)
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*this->cancel_action_ && nav_state == robot::move_base_core::State::CONTROLLING)
{
r.sleep();
ros::spinOnce();
}
geometry_msgs::PoseStamped pose;
robot_geometry_msgs::PoseStamped pose;
if (!VDA5050ClientAPI::move_base_ptr_->getRobotPose(pose))
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
@@ -808,9 +807,9 @@ namespace amr_control
return;
}
geometry_msgs::PoseStamped goal = move_base_core::offset_goal(pose, distance);
robot_geometry_msgs::PoseStamped goal = robot::move_base_core::offset_goal(pose, distance);
VDA5050ClientAPI::move_base_ptr_->moveStraightTo(goal);
geometry_msgs::Vector3 linear;
robot_geometry_msgs::Vector3 linear;
linear.x = fabs(velocity);
VDA5050ClientAPI::move_base_ptr_->setTwistLinear(linear);
linear.x = -fabs(velocity);
@@ -818,7 +817,7 @@ namespace amr_control
while (ros::ok() && !*this->cancel_action_)
{
if (nav_state == move_base_core::State::CONTROLLING)
if (nav_state == robot::move_base_core::State::CONTROLLING)
break;
r.sleep();
ros::spinOnce();
@@ -827,15 +826,15 @@ namespace amr_control
while (ros::ok() && !*this->cancel_action_)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
if (nav_state == move_base_core::State::SUCCEEDED ||
nav_state == move_base_core::State::ABORTED ||
nav_state == move_base_core::State::PREEMPTED)
if (nav_state == robot::move_base_core::State::SUCCEEDED ||
nav_state == robot::move_base_core::State::ABORTED ||
nav_state == robot::move_base_core::State::PREEMPTED)
break;
r.sleep();
ros::spinOnce();
}
if (nav_state == move_base_core::State::SUCCEEDED)
if (nav_state == robot::move_base_core::State::SUCCEEDED)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
action_state->resultDescription = "Done";
@@ -851,11 +850,11 @@ namespace amr_control
void VDA5050ClientAPI::pickUp(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
while (ros::ok() && nav_state == move_base_core::State::CONTROLLING)
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
{
r.sleep();
ros::spinOnce();
@@ -978,18 +977,18 @@ namespace amr_control
void VDA5050ClientAPI::dockToStaton(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::INITIALIZING;
ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
while (ros::ok() && nav_state == move_base_core::State::CONTROLLING)
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && nav_state == robot::move_base_core::State::CONTROLLING)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::RUNNING;
r.sleep();
ros::spinOnce();
}
if (nav_state == move_base_core::State::SUCCEEDED)
if (nav_state == robot::move_base_core::State::SUCCEEDED)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FINISHED;
action_state->resultDescription = "Done";
@@ -1003,23 +1002,23 @@ namespace amr_control
void VDA5050ClientAPI::unLoad(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{
ROS_INFO("UnLoad running");
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
{
if (nav_state == move_base_core::State::ABORTED ||
nav_state == move_base_core::State::REJECTED)
if (nav_state == robot::move_base_core::State::ABORTED ||
nav_state == robot::move_base_core::State::REJECTED)
{
if (action_state != nullptr)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
}
return;
}
if (nav_state == move_base_core::State::SUCCEEDED)
if (nav_state == robot::move_base_core::State::SUCCEEDED)
break;
r.sleep();
}
@@ -1081,23 +1080,23 @@ namespace amr_control
void VDA5050ClientAPI::load(std::shared_ptr<vda_5050::Action> action, std::shared_ptr<vda_5050::ActionState> action_state)
{
ROS_INFO("Load running");
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->nav_feedback_)
if (this->client_ptr_ && VDA5050ClientAPI::move_base_ptr_ && VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
ros::Rate r(5);
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
auto &nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
while (ros::ok() && !*enable_action_ && !*this->cancel_action_)
{
if (nav_state == move_base_core::State::ABORTED ||
nav_state == move_base_core::State::REJECTED)
if (nav_state == robot::move_base_core::State::ABORTED ||
nav_state == robot::move_base_core::State::REJECTED)
{
if (action_state != nullptr)
{
action_state->actionStatus = vda_5050::ActionState::ActionStatus::FAILED;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->feed_back_str;
action_state->resultDescription = VDA5050ClientAPI::move_base_ptr_->getFeedback()->feed_back_str;
}
return;
}
if (nav_state == move_base_core::State::SUCCEEDED)
if (nav_state == robot::move_base_core::State::SUCCEEDED)
break;
r.sleep();
}
@@ -1185,7 +1184,7 @@ namespace amr_control
auto &global_visualization = this->client_ptr_->vda5050_visualization_;
if (VDA5050ClientAPI::move_base_ptr_)
{
geometry_msgs::Pose2D robot_pose;
robot_geometry_msgs::Pose2D robot_pose;
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
{
global_visualization.agvPosition.x = robot_pose.x;
@@ -1211,9 +1210,9 @@ namespace amr_control
std::lock_guard<std::mutex> lock(this->client_ptr_->state_mutex);
auto &global_state = this->client_ptr_->vda5050_state_;
if (VDA5050ClientAPI::move_base_ptr_ != nullptr &&
VDA5050ClientAPI::move_base_ptr_->nav_feedback_ != nullptr)
VDA5050ClientAPI::move_base_ptr_->getFeedback() != nullptr)
{
geometry_msgs::Pose2D robot_pose;
robot_geometry_msgs::Pose2D robot_pose;
if (VDA5050ClientAPI::move_base_ptr_->getRobotPose(robot_pose))
{
global_state.agvPosition.x = robot_pose.x;
@@ -1221,14 +1220,14 @@ namespace amr_control
global_state.agvPosition.theta = robot_pose.theta;
}
move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->nav_feedback_->navigation_state;
if (nav_state == move_base_core::State::CONTROLLING)
robot::move_base_core::State nav_state = VDA5050ClientAPI::move_base_ptr_->getFeedback()->navigation_state;
if (nav_state == robot::move_base_core::State::CONTROLLING)
{
global_state.driving = true;
global_state.paused = false;
global_state.newBaseRequest = false;
}
else if (nav_state == move_base_core::State::PAUSED)
else if (nav_state == robot::move_base_core::State::PAUSED)
{
global_state.driving = false;
global_state.paused = true;
@@ -1254,7 +1253,7 @@ namespace amr_control
*this->pause_action_ = false;
}
std::shared_ptr<move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
std::shared_ptr<robot::move_base_core::BaseNavigation> VDA5050ClientAPI::move_base_ptr_ = nullptr;
std::shared_ptr<loc_core::BaseLocalization> VDA5050ClientAPI::loc_base_ptr_ = nullptr;
std::shared_ptr<AmrMonitor> VDA5050ClientAPI::monitor_ptr_ = nullptr;
}