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

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

578
Controllers/Packages/amr_control/src/amr_control.cpp Normal file
View File

@@ -0,0 +1,578 @@
#include "amr_control/amr_control.h"
#include <geometry_msgs/Vector3.h>
namespace amr_control
{
AmrController::AmrController(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
: initalized_(false),
vda_5050_client_api_ptr_(nullptr),
opc_ua_server_api_ptr_(nullptr),
move_base_initialized_(false),
loc_base_initialized_(false),
monitor_ptr_(nullptr),
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
this->init(nh, buffer);
}
AmrController::AmrController()
: initalized_(false),
move_base_initialized_(false),
loc_base_initialized_(false),
vda_5050_client_api_ptr_(nullptr),
opc_ua_server_api_ptr_(nullptr),
monitor_ptr_(nullptr),
loc_base_ptr_(nullptr),
move_base_ptr_(nullptr),
amr_safety_ptr_(nullptr),
move_base_loader_("move_base_core", "move_base_core::BaseNavigation"),
loc_base_loader_("loc_core", "loc_core::BaseLocalization")
{
}
AmrController::~AmrController()
{
// Wait for both threads to complete
if (move_base_thr_)
{
move_base_thr_->join();
move_base_thr_.reset();
}
if (loc_base_thr_)
{
loc_base_thr_->join();
loc_base_thr_.reset();
}
if (server_thr_)
{
server_thr_->join();
server_thr_.reset();
}
if (monitor_thr_)
{
monitor_thr_->join();
monitor_thr_.reset();
}
if (opc_ua_server_api_ptr_)
{
opc_ua_server_api_ptr_.reset();
}
if (vda_5050_client_api_ptr_)
{
vda_5050_client_api_ptr_.reset();
}
}
void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
{
if (!initalized_)
{
nh_ = nh;
tf_ = buffer;
monitor_thr_ = std::make_shared<std::thread>(
[this]()
{
this->initalizingMonitorHandle(nh_);
this->threadHandle();
});
move_base_initialized_ = false;
// Setup base localization
loc_base_thr_ = std::make_shared<std::thread>(
[this]()
{
this->initalizingLocalizationHandle(nh_);
{
std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
loc_base_initialized_ = true;
}
init_loc_base_cv_.notify_one();
});
// Setup base navigation
move_base_thr_ = std::make_shared<std::thread>(
[this]()
{
std::unique_lock<std::mutex> lock(init_loc_base_mutex_);
init_loc_base_cv_.wait(lock, [this]()
{ return loc_base_initialized_; });
this->initalizingMoveBaseHandle(nh_);
{
std::lock_guard<std::mutex> lock(init_move_base_mutex_);
move_base_initialized_ = true;
}
init_move_base_cv_.notify_one();
// this->controllerDotuff();
});
// Setup server
server_thr_ = std::make_shared<std::thread>(
[this]()
{
std::unique_lock<std::mutex> lock(init_move_base_mutex_);
init_move_base_cv_.wait(lock, [this]()
{ return move_base_initialized_; });
this->initalizingComunicationHandle(nh_);
});
ros::NodeHandle nh_core;
is_detected_maker_sub_ = nh_core.subscribe("/is_detected_maker", 1, &AmrController::isDetectedMakerCallback, this);
initalized_ = true;
}
}
void AmrController::stop()
{
}
void AmrController::initalizingComunicationHandle(ros::NodeHandle &nh)
{
this->count_ng_max_ = this->count_ok_max_ = 1;
std::shared_ptr opc_ua_server_thr = std::make_shared<std::thread>(
[this, nh]()
{
opc_ua_server_api_ptr_ =
std::make_shared<OpcUAServerAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
OpcUAServerAPI::muted_value_ = &this->muted_;
OpcUAServerAPI::unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
OpcUAServerAPI::load_excuted_ = std::bind(&AmrController::loadCallBack, this);
OpcUAServerAPI::belt_thread_ptr_ = &this->belt_thread_;
OpcUAServerAPI::cur_belt_state_en_ = &this->cur_belt_state_en_;
OpcUAServerAPI::belt_cancel_ = &this->cancel_;
OpcUAServerAPI::arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
OpcUAServerAPI::arm_thread_ptr_ = &this->arm_thread_;
OpcUAServerAPI::arm_cancel_ = &this->enable_;
OpcUAServerAPI::arm_go_home_ = &this->arm_go_home_;
OpcUAServerAPI::arm_continue_ = &this->arm_continue_;
OpcUAServerAPI::arm_power_on_ = &this->arm_power_on_;
OpcUAServerAPI::count_ng_max_ = &this->count_ng_max_;
OpcUAServerAPI::count_ok_max_ = &this->count_ok_max_;
OpcUAServerAPI::status_code_ptr_ = this->status_code_ptr_;
opc_ua_server_api_ptr_->start();
});
std::shared_ptr vda_5050_client_thr = std::make_shared<std::thread>(
[this, nh]()
{
vda_5050_client_api_ptr_ =
std::make_shared<VDA5050ClientAPI>(nh, move_base_ptr_, loc_base_ptr_, monitor_ptr_);
vda_5050_client_api_ptr_->muted_value_ = &this->muted_;
vda_5050_client_api_ptr_->unLoad_excuted_ = std::bind(&AmrController::unLoadCallBack, this);
vda_5050_client_api_ptr_->load_excuted_ = std::bind(&AmrController::loadCallBack, this);
vda_5050_client_api_ptr_->cur_belt_state_en_ = &this->cur_belt_state_en_;
vda_5050_client_api_ptr_->belt_thread_ptr_ = &this->belt_thread_;
vda_5050_client_api_ptr_->arm_function_ptr_ = std::bind(&AmrController::ArmCallBack, this);
vda_5050_client_api_ptr_->arm_thread_ptr_ = &this->arm_thread_;
vda_5050_client_api_ptr_->enable_action_ = &this->enable_;
vda_5050_client_api_ptr_->cancel_action_ = &this->cancel_;
vda_5050_client_api_ptr_->pause_action_ = &this->pause_;
vda_5050_client_api_ptr_->count_ng_max_ = &this->count_ng_max_;
vda_5050_client_api_ptr_->count_ok_max_ = &this->count_ok_max_;
});
ROS_INFO("Initalizing comunication is done");
opc_ua_server_thr->join();
vda_5050_client_thr->join();
}
void AmrController::initalizingLocalizationHandle(ros::NodeHandle &nh)
{
std::string obj_name("loc_base::LocBase");
if (tf_ == nullptr)
throw std::runtime_error("tf2_ros::Buffer object is null");
try
{
loc_base_ptr_ = loc_base_loader_.createUniqueInstance(obj_name);
ros::NodeHandle node = ros::NodeHandle(nh, "global_costmap");
loc_base_ptr_->initialize(node, tf_);
}
catch (pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
exit(1);
}
catch (std::exception &e)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
exit(1);
}
}
void AmrController::initalizingMoveBaseHandle(ros::NodeHandle &nh)
{
std::string obj_name("move_base::MoveBase");
if (tf_ == nullptr)
throw std::runtime_error("tf2_ros::Buffer object is null");
try
{
move_base_ptr_ = move_base_loader_.createUniqueInstance(obj_name);
ROS_INFO("Created object %s susseced", obj_name.c_str());
move_base_ptr_->initialize(tf_);
ros::Rate r(3);
do
{
r.sleep();
ros::spinOnce();
} while (ros::ok() && !move_base_ptr_->nav_feedback_->is_ready);
if (move_base_ptr_ != nullptr &&
move_base_ptr_->nav_feedback_ != nullptr &&
move_base_ptr_->nav_feedback_->is_ready)
{
geometry_msgs::Vector3 linear;
linear.x = 0.3;
move_base_ptr_->setTwistLinear(linear);
linear.x = -0.3;
move_base_ptr_->setTwistLinear(linear);
}
}
catch (pluginlib::PluginlibException &ex)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), ex.what());
exit(1);
}
catch (std::exception &e)
{
ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", obj_name.c_str(), e.what());
exit(1);
}
}
void AmrController::initalizingMonitorHandle(ros::NodeHandle &nh)
{
this->monitor_ptr_ = std::make_shared<AmrMonitor>(nh);
}
void AmrController::ArmCallBack()
{
ROS_INFO("Arm Calling");
std::lock_guard<std::mutex> lock(this->arm_mutex_);
{
arm_joined_ = true;
this->arm_thread_ = std::thread(&AmrController::ArmDotuff, this);
}
}
void AmrController::ArmDotuff()
{
std::shared_ptr<imr_nova_control> arm_control_ptr;
arm_control_ptr = std::make_shared<imr_nova_control>();
arm_control_ptr->enable_ = &this->enable_;
arm_control_ptr->go_home_flag_ = &this->arm_go_home_;
arm_control_ptr->continue_flag_ = &this->arm_continue_;
arm_control_ptr->power_on_flag_ = &this->arm_power_on_;
OpcUAServerAPI::mode_ptr_ = &arm_control_ptr->mode_;
this->status_code_ptr_ = reinterpret_cast<unsigned int *>(&arm_control_ptr->statusCode_);
if (!this->arm_go_home_)
{
arm_control_ptr->ok_count_max_ = &this->count_ok_max_;
arm_control_ptr->ng_count_max_ = &this->count_ng_max_;
arm_control_ptr->startModeThread();
}
else
{
arm_control_ptr->startHomeThread();
}
arm_control_ptr.reset();
ROS_INFO("Arm Finished");
std::lock_guard<std::mutex> lock(this->arm_mutex_);
this->arm_joined_ = false;
}
void AmrController::unLoadCallBack()
{
std::lock_guard<std::mutex> lock(this->arm_mutex_);
{
ROS_INFO("Shiping call");
this->belt_joined_ = true;
this->cancel_ = false;
this->cur_belt_state_en_ = amr_control::State::WAITING;
this->belt_thread_ = std::thread(&AmrController::conveyorBeltsShipping, this, std::ref(this->cur_belt_state_en_));
}
}
void AmrController::conveyorBeltsShipping(amr_control::State &state)
{
state = amr_control::State::INITIALIZING;
std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
plc_controller_ptr_->connect();
if (!plc_controller_ptr_->checkConnected())
{
state = amr_control::State::FAILED;
return;
}
if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
{
int shipping_regsister = 119, receiving_regsister = 116;
plc_controller_ptr_->setM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
ros::Duration(0.1).sleep();
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
ros::Rate r(5);
while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
{
state = amr_control::State::RUNNING;
bool output_belt[2];
plc_controller_ptr_->mulGetM(124, 125, output_belt);
bool enable_shipping = output_belt[0];
bool enable_receiving = output_belt[1];
if (!enable_shipping && !enable_receiving)
{
state = amr_control::State::FINISHED;
break;
}
r.sleep();
}
if (cancel_ || !plc_controller_ptr_->checkConnected())
state = amr_control::State::FAILED;
plc_controller_ptr_->close();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
std::lock_guard<std::mutex> lock(this->arm_mutex_);
this->belt_joined_ = false;
}
void AmrController::loadCallBack()
{
std::lock_guard<std::mutex> lock(this->arm_mutex_);
{
ROS_INFO("Receiving call");
this->belt_joined_ = true;
this->cancel_ = false;
this->cur_belt_state_en_ = amr_control::State::WAITING;
this->belt_thread_ = std::thread(&AmrController::conveyorBeltsReceiving, this, std::ref(this->cur_belt_state_en_));
}
}
void AmrController::conveyorBeltsReceiving(amr_control::State &state)
{
state = amr_control::State::INITIALIZING;
std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
plc_controller_ptr_->connect();
if (!plc_controller_ptr_->checkConnected())
{
state = amr_control::State::FAILED;
return;
}
if (plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
{
int shipping_regsister = 119, receiving_regsister = 116;
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->setM(receiving_regsister);
ros::Duration(0.1).sleep();
plc_controller_ptr_->resetM(shipping_regsister);
plc_controller_ptr_->resetM(receiving_regsister);
ros::Rate r(5);
while (ros::ok() && plc_controller_ptr_->checkConnected() && !this->cancel_)
{
state = amr_control::State::RUNNING;
bool output_belt[2];
plc_controller_ptr_->mulGetM(124, 125, output_belt);
bool enable_shipping = output_belt[0];
bool enable_receiving = output_belt[1];
if (!enable_shipping && !enable_receiving)
{
state = amr_control::State::FINISHED;
break;
}
r.sleep();
}
if (this->cancel_ || !plc_controller_ptr_->checkConnected())
state = amr_control::State::FAILED;
plc_controller_ptr_->close();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
std::lock_guard<std::mutex> lock(this->arm_mutex_);
this->belt_joined_ = false;
}
void AmrController::controllerDotuff()
{
ros::Rate r(10);
while (ros::ok())
{
std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr_;
plc_controller_ptr_ = std::make_shared<DELTA_NAMESPACE::PLC>("192.168.2.5", 502);
plc_controller_ptr_->connect();
r.sleep();
ros::spinOnce();
if (plc_controller_ptr_ == nullptr)
continue;
if (!plc_controller_ptr_->checkConnected())
continue;
this->amr_safety_ptr_ = std::make_shared<AmrSafety>();
this->amr_safety_ptr_->getController(plc_controller_ptr_);
while (ros::ok() && plc_controller_ptr_ && plc_controller_ptr_->checkConnected())
{
if (!this->monitor_ptr_)
continue;
nav_2d_msgs::Twist2D velocity;
if (this->monitor_ptr_->getVelocity(velocity))
{
cmd_vel_mtx.lock();
this->amr_safety_ptr_->safetyHandle(velocity, cmd_vel_max_, cmd_vel_recommended_);
cmd_vel_mtx.unlock();
}
if (move_base_ptr_ != nullptr &&
move_base_ptr_->nav_feedback_ != nullptr &&
move_base_ptr_->nav_feedback_->is_ready)
{
if (velocity.x <= -0.01)
this->amr_safety_ptr_->writeMutesSafety(true);
else
{
this->amr_safety_ptr_->writeMutesSafety(this->muted_);
}
}
bool have_goods;
int have_goods_regsister = 142;
if (!OpcUAServerAPI::belt_cancel_)
{
plc_controller_ptr_->getM(have_goods_regsister, have_goods);
OpcUAServerAPI::have_goods_ = have_goods;
}
else
{
plc_controller_ptr_->resetM(have_goods_regsister);
}
if (vda_5050_client_api_ptr_)
{
amr_control::OperatingMode mode_;
bool operating_mode[4];
plc_controller_ptr_->mulGetM(14, 17, operating_mode);
if (operating_mode[0])
mode_ = amr_control::OperatingMode::AUTOMATIC;
else if (operating_mode[2])
mode_ = amr_control::OperatingMode::SERVICE;
else if (operating_mode[3])
mode_ = amr_control::OperatingMode::MANUAL;
switch (mode_)
{
case amr_control::OperatingMode::AUTOMATIC:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::AUTOMATIC;
break;
case amr_control::OperatingMode::MANUAL:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::MANUAL;
break;
case amr_control::OperatingMode::SERVICE:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE;
break;
default:
vda_5050_client_api_ptr_->mode_ = vda_5050::State::OperatingMode::SERVICE; // Default
break;
}
}
r.sleep();
ros::spinOnce();
}
if (plc_controller_ptr_)
plc_controller_ptr_.reset();
if (amr_safety_ptr_)
amr_safety_ptr_.reset();
}
}
void AmrController::isDetectedMakerCallback(const std_msgs::Bool::ConstPtr &msg)
{
this->muted_ = msg->data;
}
void AmrController::threadHandle()
{
ros::Rate r(5);
while (ros::ok())
{
if (this->arm_thread_.joinable())
{
std::lock_guard<std::mutex> lock(this->arm_mutex_);
{
if (!this->arm_joined_)
{
this->arm_thread_.join();
}
}
}
if (this->belt_thread_.joinable())
{
std::lock_guard<std::mutex> lock(this->belt_mutex_);
{
if (!this->belt_joined_)
{
this->belt_thread_.join();
}
}
}
if (move_base_ptr_ != nullptr &&
move_base_ptr_->nav_feedback_ != nullptr &&
move_base_ptr_->nav_feedback_->is_ready)
{
nav_2d_msgs::Twist2D velocity;
if (this->monitor_ptr_ && this->monitor_ptr_->getVelocity(velocity))
{
this->cmd_vel_max_.x = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, std::max(OpcUAServerAPI::cmd_vel_max_.x, vda_5050_client_api_ptr_->cmd_vel_max_.x));
this->cmd_vel_max_.theta = !vda_5050_client_api_ptr_ ? 0.3 : std::max(0.3, vda_5050_client_api_ptr_->cmd_vel_max_.theta);
geometry_msgs::Vector3 linear;
geometry_msgs::Vector3 angular;
cmd_vel_mtx.lock();
linear.x = this->amr_safety_ptr_ != nullptr ? this->cmd_vel_recommended_.x : this->cmd_vel_max_.x;
angular.z = this->cmd_vel_max_.theta;
cmd_vel_mtx.unlock();
this->move_base_ptr_->setTwistLinear(linear);
linear.x *= -1.0;
this->move_base_ptr_->setTwistLinear(linear);
this->move_base_ptr_->setTwistAngular(angular);
}
}
r.sleep();
ros::spinOnce();
}
}
}

27
Controllers/Packages/amr_control/src/amr_control_node.cpp Normal file
View File

@@ -0,0 +1,27 @@
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include "amr_control/amr_control.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "move_base_node");
ros::start();
ROS_INFO("ros::this_node::getName() : %s", ros::this_node::getName().c_str());
ros::NodeHandle nh(ros::this_node::getName());
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>();
tf2_ros::TransformListener tf2(*buffer);
try
{
std::shared_ptr<amr_control::AmrController> amr_ctr_;
amr_ctr_ = std::make_shared<amr_control::AmrController>(nh, buffer);
ros::spin();
if(amr_ctr_ != nullptr)
amr_ctr_->stop();
amr_ctr_.reset();
}
catch (const std::exception& exc)
{
std::cout << exc.what() << std::endl;
}
return 0;
}

712
Controllers/Packages/amr_control/src/amr_make_plan_with_order.cpp Normal file
View File

@@ -0,0 +1,712 @@
#include "amr_control/amr_make_plan_with_order.h"
namespace amr_control
{
bool MakePlanWithOrder(const vda_5050::Order order, bool is_move_backward, uint8_t &status, std::string &message, std::vector<vda_5050::Pose> &posesOnPathWay)
{
std::map<std::string, vda_5050::Node> orderNodes;
Spline_Inf *input_spline_inf = new Spline_Inf();
CurveCommon *curve_design = new CurveCommon();
posesOnPathWay.clear();
std::map<std::string, vda_5050::Node> map_str;
if ((int)order.nodes.size() == 0 || (int)order.edges.size() == 0)
{
status = 1;
message = "Nodes or Edges in Order is empty";
return false;
}
for (auto &node : order.nodes)
{
orderNodes.insert({node.nodeId, node});
}
for (int i = 0; i < (int)order.edges.size(); i++)
{
auto start_nodeId_it = orderNodes.find(order.edges[i].startNodeId);
auto end_nodeId_it = orderNodes.find(order.edges[i].endNodeId);
if (start_nodeId_it != orderNodes.end() && end_nodeId_it != orderNodes.end())
{
std::vector<vda_5050::Pose> posesOnEdge;
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> control_points;
std::vector<double> knot_vector;
std::vector<double> weight_vector;
int degree = 0;
int order_ = 0;
control_points.reserve(order.edges[i].trajectory.controlPoints.size());
knot_vector.reserve(order.edges[i].trajectory.knotVector.size());
weight_vector.reserve(order.edges[i].trajectory.controlPoints.size());
for (int j = 0; j < (int)order.edges[i].trajectory.controlPoints.size(); j++)
{
control_points.push_back(Eigen::Vector3d(order.edges[i].trajectory.controlPoints[j].x, order.edges[i].trajectory.controlPoints[j].y, 0));
weight_vector.push_back(order.edges[i].trajectory.controlPoints[j].weight);
}
for (int k = 0; k < (int)order.edges[i].trajectory.knotVector.size(); k++)
{
knot_vector.push_back(order.edges[i].trajectory.knotVector[k]);
}
degree = (int)order.edges[i].trajectory.degree;
if (curveIsValid(degree, knot_vector, control_points))
{
double step = 0.01;
order_ = degree + 1;
input_spline_inf->control_point.clear();
input_spline_inf->knot_vector.clear();
input_spline_inf->weight.clear();
curve_design->ReadSplineInf(input_spline_inf, order_, control_points, knot_vector);
curve_design->ReadSplineInf(input_spline_inf, weight_vector, false);
for (double u_test = 0; u_test <= 1; u_test += step)
{
geometry_msgs::Point curve_point;
curve_point = curve_design->CalculateCurvePoint(input_spline_inf, u_test, true);
if (!std::isnan(curve_point.x) && !std::isnan(curve_point.y))
posesOnEdge.push_back(vda_5050::Pose(curve_point.x, curve_point.y, 0));
// ROS_INFO("curve_point: %f, %f at u: %f",curve_point.x, curve_point.y, u_test);
}
if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
{
// if startNode of this edge and start element of posesOnEdge are the different point
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
{
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
}
// if endNode of this edge and end element of posesOnEdge are the different point
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
{
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
}
if (!posesOnPathWay.empty())
{
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626)
{
setYawAllPosesOnEdge(posesOnEdge, false);
}
else if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276)
{
setYawAllPosesOnEdge(posesOnEdge, true);
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else
{
if (is_move_backward == false)
{
setYawAllPosesOnEdge(posesOnEdge, false);
}
else
{
setYawAllPosesOnEdge(posesOnEdge, true);
}
}
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
{
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
{
if (i != 1)
{ // don't check angle of edge 1
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
{
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else
{
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
}
}
else
{
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else // posesOnPathWay is empty
{
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
}
}
else if (!isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
{
// if startNode of this edge and start element of posesOnEdge are the different point
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
{
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, 0.0123443210));
}
// if endNode of this edge and end element of posesOnEdge are the different point
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
{
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
}
if (i == ((int)order.edges.size() - 1))
{
if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 1.5707963268) // <= 90 degree
{
setYawAllPosesOnEdge(posesOnEdge, false);
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
else
{
setYawAllPosesOnEdge(posesOnEdge, true);
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
}
else
{
if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626) // <= 50 degree
{
setYawAllPosesOnEdge(posesOnEdge, false);
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
else if (computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276) // >= 130 degree
{
setYawAllPosesOnEdge(posesOnEdge, true);
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
{
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
{
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
{
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else
{
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else // posesOnPathWay is empty
{
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
}
}
else if (isThetaValid(orderNodes[order.edges[i].startNodeId].nodePosition.theta) && !isThetaValid(orderNodes[order.edges[i].endNodeId].nodePosition.theta))
{
// if startNode of this edge and start element of posesOnEdge are the different point
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
{
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
}
// if endNode of this edge and end element of posesOnEdge are the different point
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
{
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, 0.0123443210));
}
if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626) // <= 50 degree)
{
setYawAllPosesOnEdge(posesOnEdge, false);
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
}
else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276) // >= 130 degree
{
setYawAllPosesOnEdge(posesOnEdge, true);
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
{
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
{
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
{
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else
{
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else // posesOnPathWay is empty
{
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
}
}
else
{
// if startNode of this edge and start element of posesOnEdge are the different point
if (orderNodes[order.edges[i].startNodeId].nodePosition.x != posesOnEdge.front().getX() ||
orderNodes[order.edges[i].startNodeId].nodePosition.y != posesOnEdge.front().getY())
{
posesOnEdge.insert(posesOnEdge.begin(), vda_5050::Pose(orderNodes[order.edges[i].startNodeId].nodePosition.x, orderNodes[order.edges[i].startNodeId].nodePosition.y, orderNodes[order.edges[i].startNodeId].nodePosition.theta));
}
// if endNode of this edge and end element of posesOnEdge are the different point
if (orderNodes[order.edges[i].endNodeId].nodePosition.x != posesOnEdge.back().getX() ||
orderNodes[order.edges[i].endNodeId].nodePosition.y != posesOnEdge.back().getY())
{
posesOnEdge.insert(posesOnEdge.end(), vda_5050::Pose(orderNodes[order.edges[i].endNodeId].nodePosition.x, orderNodes[order.edges[i].endNodeId].nodePosition.y, orderNodes[order.edges[i].endNodeId].nodePosition.theta));
}
// DeltaAngleStart <= 50 degree and DeltaAngleEnd <= 50 degree
if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) <= 0.872664626 &&
computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) <= 0.872664626)
{
setYawAllPosesOnEdge(posesOnEdge, false);
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
// DeltaAngleStart >= 130 degree and DeltaAngleEnd >= 130 degree
else if (computeDeltaAngleStartNode(orderNodes[order.edges[i].startNodeId].nodePosition.theta, posesOnEdge.front(), posesOnEdge[1]) >= 2.2689280276 &&
computeDeltaAngleEndNode(orderNodes[order.edges[i].endNodeId].nodePosition.theta, posesOnEdge.back(), posesOnEdge[posesOnEdge.size() - 2]) >= 2.2689280276)
{
setYawAllPosesOnEdge(posesOnEdge, true);
posesOnEdge.front().setYaw(orderNodes[order.edges[i].startNodeId].nodePosition.theta); // set yaw angle of the start pose to startNode theta
posesOnEdge.back().setYaw(orderNodes[order.edges[i].endNodeId].nodePosition.theta); // set yaw angle of the end pose to endNode theta
}
if (!posesOnPathWay.empty()) // posesOnPathWay has datas
{
if (posesOnEdge.front().getX() == posesOnPathWay.back().getX() &&
posesOnEdge.front().getY() == posesOnPathWay.back().getY())
{
if (computeDeltaAngleStartNode(posesOnPathWay.back().getYaw(), posesOnEdge.front().getYaw(), posesOnEdge.front()) <= 0.872664626) // <= 50 degree
{
// if yaw angle of the end pose in posesOnPathWay is default, set it to yaw angle of start pose in posesOnEdge
posesOnPathWay.back().setYaw(posesOnEdge.front().getYaw());
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin() + 1, posesOnEdge.end());
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNode: %s, endNode: %s is not good",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else
{
ROS_WARN("Trajectory of Edge: %s. startNode: %s has posision invalid",
order.edges[i].edgeId.c_str(), order.edges[i].startNodeId.c_str());
status = 3;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is not good";
return false;
break;
}
}
else // posesOnPathWay is empty
{
posesOnPathWay.insert(posesOnPathWay.end(), posesOnEdge.begin(), posesOnEdge.end());
}
}
}
else
{
ROS_WARN("Trajectory of Edge: %s, startNodeId: %s, endNodeId: %s is invalid", order.edges[i].edgeId.c_str(),
order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 2;
message = "Trajectory of Edge: " + order.edges[i].edgeId + ", startNode: " + order.edges[i].startNodeId.c_str() +
", endNode: " + order.edges[i].endNodeId.c_str() + " is invalid NURBS-curve";
return false;
break;
}
}
else
{
ROS_WARN("Edge: %s not found startNodeId: %s or endNodeId: %s", order.edges[i].edgeId.c_str(),
order.edges[i].startNodeId.c_str(), order.edges[i].endNodeId.c_str());
status = 1;
message = "Edge: " + order.edges[i].edgeId + " not found startNodeId: " + order.edges[i].startNodeId.c_str() +
" or endNodeId: " + order.edges[i].endNodeId.c_str();
return false;
break;
}
// ROS_INFO("Finish to compute at Edge[%d]: %s", i, order.edges[i].edgeDescription.c_str());
}
status = 0;
message = "Success to make plan: StartNode: " + order.edges[0].startNodeId + ", EndNode: " + order.edges[order.edges.size() - 1].endNodeId;
if (input_spline_inf)
delete (input_spline_inf);
if (curve_design)
delete (curve_design);
return true;
}
bool isThetaValid(double theta)
{
bool result = false;
if (theta < -M_PI || theta > M_PI)
result = false;
else
result = true;
return result;
}
bool curveIsValid(int degree, const std::vector<double> &knot_vector,
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &control_points)
{
if (degree < 1 || degree > 9)
{
ROS_WARN("degree is invalid value");
return false;
}
if (!((knot_vector.size() - degree - 1) == control_points.size()))
{
ROS_WARN("relation between degree, number of knots, and number of control points is invalid");
return false;
}
// if(std::is_sorted(knot_vector.begin(), knot_vector.end()))
// {
// ROS_WARN("knot vector is not monotonic");
// return false;
// }
return true;
}
double computeDeltaAngleStartNode(double theta, vda_5050::Pose &startPose, vda_5050::Pose &next_Pose)
{
double delta_angle = 0;
if (isThetaValid(theta))
{
double xAB = next_Pose.getX() - startPose.getX();
double yAB = next_Pose.getY() - startPose.getY();
double d = sqrt(xAB * xAB + yAB * yAB);
double xC = startPose.getX() + d * cos(theta);
double yC = startPose.getY() + d * sin(theta);
double xAC = xC - startPose.getX();
double yAC = yC - startPose.getY();
double dAB = sqrt(xAB * xAB + yAB * yAB);
double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("xC: %f, yC: %f", xC, yC);
// ROS_WARN("dAB: %f", dAB);
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
double computeDeltaAngleEndNode(double theta, vda_5050::Pose &endPose, vda_5050::Pose &prev_Pose)
{
double delta_angle = 0;
if (isThetaValid(theta))
{
double xAB = endPose.getX() - prev_Pose.getX();
double yAB = endPose.getY() - prev_Pose.getY();
double d = sqrt(xAB * xAB + yAB * yAB);
double xC = endPose.getX() + d * cos(theta);
double yC = endPose.getY() + d * sin(theta);
double xBC = xC - endPose.getX();
double yBC = yC - endPose.getY();
double dAB = sqrt(xAB * xAB + yAB * yAB);
double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("xC: %f, yC: %f", xC, yC);
// ROS_WARN("dAB: %f", dAB);
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
double computeDeltaAngleEndOfPlan(double theta, geometry_msgs::Pose &endPose, geometry_msgs::Pose &prev_Pose)
{
double delta_angle = 0;
if (isThetaValid(theta))
{
double xAB = endPose.position.x - prev_Pose.position.x;
double yAB = endPose.position.y - prev_Pose.position.y;
double d = sqrt(xAB * xAB + yAB * yAB);
double xC = endPose.position.x + d * cos(theta);
double yC = endPose.position.y + d * sin(theta);
double xBC = xC - endPose.position.x;
double yBC = yC - endPose.position.y;
double dAB = sqrt(xAB * xAB + yAB * yAB);
double cos_a = (xAB * xBC + yAB * yBC) / (dAB * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("xC: %f, yC: %f", xC, yC);
// ROS_WARN("dAB: %f", dAB);
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
void setYawAllPosesOnEdge(std::vector<vda_5050::Pose> &posesOnEdge, bool reverse)
{
if (!reverse)
{
if (!posesOnEdge.empty())
{
if (posesOnEdge.size() > 2)
{
for (int i = 0; i < ((int)posesOnEdge.size() - 1); i++)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i + 1].getX(), posesOnEdge[i + 1].getY());
posesOnEdge[i].setYaw(theta);
}
posesOnEdge.back().setYaw(posesOnEdge[posesOnEdge.size() - 2].getYaw());
}
else if (posesOnEdge.size() == 2)
{
if (posesOnEdge[0].getX() != posesOnEdge[1].getX())
{
double theta = calculateAngle(posesOnEdge[0].getX(), posesOnEdge[0].getY(),
posesOnEdge[1].getX(), posesOnEdge[1].getY());
posesOnEdge[0].setYaw(theta);
posesOnEdge[1].setYaw(theta);
}
}
}
}
else
{
if (!posesOnEdge.empty())
{
if (posesOnEdge.size() > 2)
{
for (int i = (int)posesOnEdge.size() - 1; i > 0; i--)
{
double theta = calculateAngle(posesOnEdge[i].getX(), posesOnEdge[i].getY(),
posesOnEdge[i - 1].getX(), posesOnEdge[i - 1].getY());
posesOnEdge[i].setYaw(theta);
}
posesOnEdge.front().setYaw(posesOnEdge[1].getYaw());
}
else if (posesOnEdge.size() == 2)
{
if (posesOnEdge[1].getX() != posesOnEdge[0].getX())
{
double theta = calculateAngle(posesOnEdge[1].getX(), posesOnEdge[1].getY(),
posesOnEdge[0].getX(), posesOnEdge[0].getY());
posesOnEdge[1].setYaw(theta);
posesOnEdge[0].setYaw(theta);
}
}
}
}
}
double computeDeltaAngleStartNode(double thetaEnd, double thetaStart, vda_5050::Pose &Pose)
{
double delta_angle = 0;
if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
{
double d = 1;
double xA = Pose.getX();
double yA = Pose.getY();
double xB = xA + d * cos(thetaEnd);
double yB = yA + d * sin(thetaEnd);
double xAB = xB - xA;
double yAB = yB - yA;
double xC = xA + d * cos(thetaStart);
double yC = yA + d * sin(thetaStart);
double xAC = xC - xA;
double yAC = yC - yA;
double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
double computeDeltaAngleStartOfPlan(double theta, geometry_msgs::Pose &startPose, geometry_msgs::Pose &next_Pose)
{
double delta_angle = 0;
if (isThetaValid(theta))
{
double xAB = next_Pose.position.x - startPose.position.x;
double yAB = next_Pose.position.y - startPose.position.y;
double d = sqrt(xAB * xAB + yAB * yAB);
double xC = startPose.position.x + d * cos(theta);
double yC = startPose.position.y + d * sin(theta);
double xAC = xC - startPose.position.x;
double yAC = yC - startPose.position.y;
double dAB = sqrt(xAB * xAB + yAB * yAB);
double cos_a = (xAB * xAC + yAB * yAC) / (dAB * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("xC: %f, yC: %f", xC, yC);
// ROS_WARN("dAB: %f", dAB);
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
double computeDeltaAngleStartOfPlan(double thetaEnd, double thetaStart, geometry_msgs::Pose &Pose)
{
double delta_angle = 0;
if (isThetaValid(thetaEnd) && isThetaValid(thetaStart))
{
double d = 1;
double xA = Pose.position.x;
double yA = Pose.position.y;
double xB = xA + d * cos(thetaEnd);
double yB = yA + d * sin(thetaEnd);
double xAB = xB - xA;
double yAB = yB - yA;
double xC = xA + d * cos(thetaStart);
double yC = yA + d * sin(thetaStart);
double xAC = xC - xA;
double yAC = yC - yA;
double cos_a = (xAB * xAC + yAB * yAC) / (d * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
delta_angle = acos(cos_a);
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("delta_angle: %f", delta_angle);
}
return delta_angle;
}
double calculateDistance(vda_5050::Pose &pose1, vda_5050::Pose &pose2)
{
double dx = pose1.getX() - pose2.getX();
double dy = pose1.getY() - pose2.getY();
return std::sqrt(dx * dx + dy * dy);
}
double computeDeltaAngle(vda_5050::Pose &Pose1, vda_5050::Pose &Pose2)
{
double delta_angle = 0;
double xA = Pose1.getX();
double yA = Pose1.getY();
double xB = Pose2.getX();
double yB = Pose2.getY();
double xAB = xB - xA;
double yAB = yB - yA;
double d = sqrt(xAB * xAB + yAB * yAB);
double xC = xB + d * cos(Pose2.getYaw());
double yC = yB + d * sin(Pose2.getYaw());
double xBC = xC - xB;
double yBC = yC - yB;
double cos_a = (xAB * xBC + yAB * yBC) / (d * d);
if (cos_a > 1)
cos_a = 1;
else if (cos_a < (-1))
cos_a = -1;
double delta_angle_tmp = acos(cos_a);
if (delta_angle_tmp >= 1.5707963268)
{
delta_angle = M_PI - delta_angle_tmp;
}
else
{
delta_angle = delta_angle_tmp;
}
// delta_angle = delta_angle*180/M_PI;
// ROS_WARN("delta_angle: %f", delta_angle);
return delta_angle;
}
}

37
Controllers/Packages/amr_control/src/amr_monitor.cpp Normal file
View File

@@ -0,0 +1,37 @@
#include "amr_control/amr_monitor.h"
namespace amr_control
{
AmrMonitor::AmrMonitor() {}
AmrMonitor::AmrMonitor(const ros::NodeHandle &nh)
{
if (!initalized_)
this->init(nh);
}
AmrMonitor::~AmrMonitor()
{
if (odom_sub_)
odom_sub_.reset();
}
void AmrMonitor::init(const ros::NodeHandle &nh)
{
if (!initalized_)
{
ros::NodeHandle nh_core;
odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh_core);
initalized_ = true;
}
}
bool AmrMonitor::getVelocity(nav_2d_msgs::Twist2D &velocity)
{
if (!odom_sub_)
return false;
else
velocity = odom_sub_->getTwist();
return true;
}
}

3491
Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

85
Controllers/Packages/amr_control/src/amr_safety.cpp Normal file
View File

@@ -0,0 +1,85 @@
#include "amr_control/amr_safety.h"
namespace amr_control
{
AmrSafety::AmrSafety() {}
AmrSafety::AmrSafety(const ros::NodeHandle &nh)
{
if (!initalized_)
this->init(nh);
}
AmrSafety::~AmrSafety() {}
void AmrSafety::init(const ros::NodeHandle &nh)
{
if (!initalized_)
{
initalized_ = true;
}
}
bool AmrSafety::getController(std::shared_ptr<DELTA_NAMESPACE::PLC> plc_controller_ptr)
{
if (plc_controller_ptr == nullptr)
return false;
else
{
plc_controller_ptr_ = plc_controller_ptr;
return true;
}
}
void AmrSafety::safetyHandle(
nav_2d_msgs::Twist2D velocity, nav_2d_msgs::Twist2D cmd_vel_max, nav_2d_msgs::Twist2D &cmd_vel)
{
if (plc_controller_ptr_ == nullptr)
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
else if (!plc_controller_ptr_->checkConnected())
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
else
{
nav_2d_msgs::Twist2D velocity_warn;
velocity_warn.x = 0.15;
velocity_warn.theta = 0.5;
nav_2d_msgs::Twist2D velocity_stop;
// Dieu khien chuyen vung giam toc va mute
bool zone_warn_cases;
if (velocity.x >= velocity_warn.x)
zone_warn_cases = false;
else if (velocity.x < velocity_warn.x && velocity.x > 0.05)
zone_warn_cases = true;
plc_controller_ptr_->writeM(52, zone_warn_cases);
// Nhan tin hieu 3 vung tu lidar
bool output_lidar[4];
plc_controller_ptr_->mulGetM(30, 33, output_lidar);
if (output_lidar[0] && output_lidar[1] && output_lidar[2])
cmd_vel = velocity_stop;
else if (output_lidar[0] && !output_lidar[1] && output_lidar[2])
cmd_vel = velocity_warn;
else if (output_lidar[0] && !output_lidar[1] && !output_lidar[2])
cmd_vel = cmd_vel_max;
else
cmd_vel = velocity_stop;
}
}
void AmrSafety::writeMutesSafety(bool value)
{
if (plc_controller_ptr_ == nullptr)
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller is null");
else if (!plc_controller_ptr_->checkConnected())
ROS_WARN_THROTTLE(1.0, "[AmrSafety] - PLC controller was not conntected");
else
{
bool f_data;
plc_controller_ptr_->getM(53, f_data);
if (f_data != value)
plc_controller_ptr_->writeM(53, value);
}
}
}

1260
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp Normal file
View File

File diff suppressed because it is too large Load Diff