|
|
@@ -80,6 +80,7 @@ bool Navigation::Init(const NavParameter::Navigation_parameter ¶meter) {
|
|
|
return false;
|
|
|
}
|
|
|
monitor_->set_speedcmd_topic(agv_p.pubspeedtopic());
|
|
|
+ monitor_->set_clampLifterCmd_topic(agv_p.pubclampliftertopic());
|
|
|
monitor_->AddCallback(agv_p.subposetopic(), Navigation::RobotPoseCallback, this);
|
|
|
monitor_->AddCallback(agv_p.subspeedtopic(), Navigation::RobotSpeedCallback, this);
|
|
|
}
|
|
|
@@ -331,6 +332,7 @@ bool Navigation::SlowlyStop() {
|
|
|
|
|
|
|
|
|
Navigation::Navigation() {
|
|
|
+ wheelBase_ = 0;
|
|
|
isInSpace_ = false; //是否在车位或者正在进入车位
|
|
|
RWheel_position_ = eUnknow;
|
|
|
move_mode_ = eSingle;
|
|
|
@@ -565,22 +567,25 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
|
|
|
enableTotarget = x_enable;
|
|
|
}
|
|
|
//巡线无法到达目标点
|
|
|
- if (enableTotarget == false) {
|
|
|
+ Pose2d target(node.x(), node.y(), 0);
|
|
|
+ Pose2d targetInCurrent=Pose2d::relativePose(target,current);
|
|
|
+
|
|
|
+ if (enableTotarget == false && Pose2d::distance(Pose2d(0,0,0),targetInCurrent)>0.3) {
|
|
|
if (already_in_mpc && enable_rotate == false) {
|
|
|
printf(" Move to node failed ,impossible to target and rotate is not enabled\n");
|
|
|
return false;
|
|
|
}
|
|
|
- Pose2d target(node.x(), node.y(), 0);
|
|
|
-// if (RotateReferToTarget(target, limit_w, anyDirect) == false) {
|
|
|
+
|
|
|
int directions = Direction::eForward;
|
|
|
if (anyDirect)
|
|
|
directions = Direction::eForward | Direction::eBackward |
|
|
|
Direction::eLeft | Direction::eRight;
|
|
|
- if (RotateReferToTarget(node, limit_w, directions) != eMpcSuccess) {
|
|
|
- std::cout << " Rotate refer to target failed,target: " << target <<
|
|
|
- " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl;
|
|
|
- return false;
|
|
|
- }
|
|
|
+
|
|
|
+ if (RotateReferToTarget(node, limit_w, directions) != eMpcSuccess) {
|
|
|
+ std::cout << " Rotate refer to target failed,target: " << target <<
|
|
|
+ " directions: " << std::hex << directions << " line_: " << __LINE__ << std::endl;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
continue;
|
|
|
}
|
|
|
already_in_mpc = true;
|
|
|
@@ -602,7 +607,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
-bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
+bool Navigation::execute_nodes(NavMessage::NewAction action,int space_id) {
|
|
|
if (action.type() == 3 || action.type() == 4) //最优动作导航 or 导航中保证朝向严格一致
|
|
|
{
|
|
|
if (!parameter_.has_nodeangularlimit() || !parameter_.has_nodevelocitylimit() || action.pathnodes_size() == 0) {
|
|
|
@@ -615,18 +620,36 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
|
|
|
stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
|
|
|
|
|
|
+ /////////////////////////////////////////
|
|
|
+
|
|
|
+ LifterStatus lifter_actType;
|
|
|
+ std::thread* thread_lifter= nullptr;
|
|
|
+ if (space_id>99 && space_id< 1000){
|
|
|
+ //进载车板库前提升机构上升
|
|
|
+ lifter_actType=eRose;
|
|
|
+ thread_lifter=new std::thread(&Navigation::lifter_rise,this);
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ //否则下降
|
|
|
+ lifter_actType=eDowned;
|
|
|
+ thread_lifter=new std::thread(&Navigation::lifter_down,this);
|
|
|
+ }
|
|
|
+
|
|
|
+ ////////////////////////////////////////////////////////////
|
|
|
+
|
|
|
+
|
|
|
for (int i = 0; i < action.pathnodes_size(); ++i) {
|
|
|
NavMessage::PathNode node = action.pathnodes(i);
|
|
|
if (i + 1 < action.pathnodes_size()) {
|
|
|
NavMessage::PathNode next = action.pathnodes(i + 1);
|
|
|
Pose2d vec(next.x() - node.x(), next.y() - node.y(), 0);
|
|
|
node.set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
|
|
|
- node.set_l(0.05);
|
|
|
- node.set_w(0.10);
|
|
|
+ node.set_l(0.03);
|
|
|
+ node.set_w(0.20);
|
|
|
} else {
|
|
|
node.set_theta(0);
|
|
|
- node.set_l(0.03);
|
|
|
- node.set_w(0.10);
|
|
|
+ node.set_l(0.02);
|
|
|
+ node.set_w(0.20);
|
|
|
}
|
|
|
int directions = Direction::eForward;
|
|
|
if (anyDirect)
|
|
|
@@ -643,8 +666,22 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
else
|
|
|
isInSpace_ = false;
|
|
|
}
|
|
|
+
|
|
|
+ while(timed_lifter_.Get()!=lifter_actType && cancel_==false){
|
|
|
+ usleep(1000*100);
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ printf(" wait lifter action:%d\n",lifter_actType);
|
|
|
+ thread_lifter->join();
|
|
|
+ printf("lifter action:%d completed!!!\n",lifter_actType);
|
|
|
+ delete thread_lifter;
|
|
|
+ if(cancel_==true){
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
return true;
|
|
|
}
|
|
|
+
|
|
|
printf("execute PathNode failed ,action type is not 3/4 :%d\n", action.type());
|
|
|
return false;
|
|
|
}
|
|
|
@@ -708,22 +745,22 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
|
|
|
stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
|
|
|
double acc_angular = 20 * M_PI / 180.0;
|
|
|
double dt = 0.1;
|
|
|
- Pose2d rotated = timedPose_.Get();
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
double x = space.x();
|
|
|
double y = space.y();
|
|
|
- Pose2d vec(x - rotated.x(), y - rotated.y(), 0);
|
|
|
- rotated.mutable_theta() = Pose2d::vector2yaw(vec.x(), vec.y());
|
|
|
+ Pose2d vec(x - current.x(), y - current.y(), 0);
|
|
|
+ double vecTheta = Pose2d::vector2yaw(vec.x(), vec.y());
|
|
|
bool isFront=isFront_.Get();
|
|
|
- printf(" front__:%d theta:%f\n",isFront,rotated.theta());
|
|
|
- if(rotated.theta()<0.)
|
|
|
+ printf(" front__:%d theta:%f\n",isFront,vecTheta);
|
|
|
+ if(vecTheta<0.)
|
|
|
isFront=!isFront;
|
|
|
|
|
|
- //前车先到,后车进入2点,保持与前车一致的朝向
|
|
|
+ //如果是后车，计算目标点（车位点-轴距）
|
|
|
if (isFront==false ) {
|
|
|
printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
|
|
|
if (move_mode_ == eSingle) {
|
|
|
- x -= wheelbase * cos(rotated.theta());
|
|
|
- y -= wheelbase * sin(rotated.theta());
|
|
|
+ x -= wheelbase * cos(vecTheta);
|
|
|
+ y -= wheelbase * sin(vecTheta);
|
|
|
printf("确定车位点：eSingle\n");
|
|
|
}
|
|
|
} else { //当后车先到,倒车入库
|
|
|
@@ -731,12 +768,13 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
|
|
|
//rotated.mutable_theta() = space.theta();
|
|
|
//rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
|
|
|
}
|
|
|
- if(rotated.theta()<0.)
|
|
|
- rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
|
|
|
+
|
|
|
+ Pose2d spaceInCurrent=Pose2d::relativePose(Pose2d(space.x(),space.y(),0),current);
|
|
|
+
|
|
|
|
|
|
target.set_x(x);
|
|
|
target.set_y(y);
|
|
|
- target.set_theta(rotated.theta());
|
|
|
+ target.set_theta(current.theta());
|
|
|
target.set_l(0.02);
|
|
|
target.set_w(0.05);
|
|
|
target.set_id(space.id());
|
|
|
@@ -753,8 +791,10 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
|
|
|
|
|
|
while (cancel_ == false) {
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
- Pose2d current = timedPose_.Get();
|
|
|
- double yawDiff = (rotated - current).theta();
|
|
|
+ Pose2d current_now = timedPose_.Get();
|
|
|
+ if(spaceInCurrent.x()<0.)
|
|
|
+ current_now = current_now.rotate(current_now.x(), current_now.y(), M_PI);
|
|
|
+ double yawDiff = vecTheta - current_now.theta();
|
|
|
|
|
|
//一次变速
|
|
|
std::vector<double> out;
|
|
|
@@ -814,11 +854,12 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
|
|
|
0);
|
|
|
action.mutable_streetnode()->set_l(0.02);
|
|
|
- action.mutable_streetnode()->set_w(0.05);
|
|
|
+ action.mutable_streetnode()->set_w(0.2);
|
|
|
action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
|
|
|
/*
|
|
|
- * 是否需要添加判断，距离较远才先运动到马路点==============================？？？？？？？？？
|
|
|
+ * 是否需要添加判断，距离较远才先运动到马路点=============================
|
|
|
*/
|
|
|
+
|
|
|
if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, true) == false) {
|
|
|
printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
|
|
|
return false;
|
|
|
@@ -832,27 +873,11 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
|
|
|
return false;
|
|
|
}
|
|
|
-
|
|
|
- std::string id = action.spacenode().id().substr(1,action.spacenode().id().length()-1);
|
|
|
- int space_id = stoi(id);
|
|
|
- if (space_id>99 && space_id< 1000){
|
|
|
- //进载车板库前提升机构上升
|
|
|
- if (lifter_rise() == false){
|
|
|
- printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
|
|
|
- }
|
|
|
- }
|
|
|
- else{
|
|
|
- //否则下降
|
|
|
- if (lifter_down() == false){
|
|
|
- printf(" Enter space | lifter down failed. line:%d\n",__LINE__);
|
|
|
- }
|
|
|
-
|
|
|
+ if(move_mode_==eSingle){
|
|
|
+ if(!clamp_fully_open())
|
|
|
+ return false;
|
|
|
}
|
|
|
|
|
|
- if (space_id == 1101){
|
|
|
- //单车进入口搬车，夹持杆打开到全开位
|
|
|
-
|
|
|
- }
|
|
|
//入库
|
|
|
Pose2d rotated = timedPose_.Get();
|
|
|
double x = new_target.x();
|
|
|
@@ -890,23 +915,14 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
if (diff.x() < 0.05 || diff.y() < 0.10) {
|
|
|
//出库，移动到马路点，允许自由方向，不允许中途旋转
|
|
|
action.mutable_streetnode()->set_theta(theta);
|
|
|
- action.mutable_streetnode()->set_l(0.04);
|
|
|
- action.mutable_streetnode()->set_w(0.06);
|
|
|
+ action.mutable_streetnode()->set_l(0.03);
|
|
|
+ action.mutable_streetnode()->set_w(0.2);
|
|
|
std::cout << " Out space target street node:" << space << std::endl;
|
|
|
if (MoveToTarget(action.streetnode(), limit_v, limit_w, true, false) == false) {
|
|
|
printf(" out space failed type:%d: MoveTo StreetNode failed\n", action.type());
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- //出库后提升机构下降
|
|
|
- if (lifter_down() == false){
|
|
|
- printf(" Out space | lifter down failed. line:%d\n",__LINE__);
|
|
|
- }
|
|
|
-// if(RotateAfterOutSpace()!=eMpcSuccess) {
|
|
|
-// printf(" after out space rotate Failed!!!\n");
|
|
|
-// return false;
|
|
|
-// }
|
|
|
-// LogWriter("Out space end-");
|
|
|
} else {
|
|
|
std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
|
|
|
<< std::endl;
|
|
|
@@ -1089,7 +1105,7 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
|
|
|
double b = (x - tx) * sin(theta) + (y - ty) * cos(theta);
|
|
|
|
|
|
if (pow(a / l, 8) + pow(b / w, 8) <= 1) {
|
|
|
- if (fabs(timedV_.Get()) < 0.06 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(timedV_.Get()) < 0.05 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
printf(" Arrived target: %f %f l:%f w:%f theta:%f\n", tx, ty, l, w, theta);
|
|
|
return true;
|
|
|
}
|
|
|
@@ -1169,21 +1185,21 @@ bool Navigation::clamp_close() {
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
-bool Navigation::clamp_open() {
|
|
|
+bool Navigation::clamp_half_open() {
|
|
|
if (monitor_) {
|
|
|
printf("Clamp openning...\n");
|
|
|
- monitor_->clamp_open(move_mode_);
|
|
|
- actionType_ = eClampOpen;
|
|
|
+ monitor_->clamp_half_open(move_mode_);
|
|
|
+ actionType_ = eClampHalfOpen;
|
|
|
while (cancel_ == false) {
|
|
|
if (timed_clamp_.timeout()) {
|
|
|
printf("timed clamp is timeout\n");
|
|
|
return false;
|
|
|
}
|
|
|
- if (timed_clamp_.Get() == eOpened)
|
|
|
+ if (timed_clamp_.Get() == eHalfOpened)
|
|
|
return true;
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- monitor_->clamp_open(move_mode_);
|
|
|
- actionType_ = eClampOpen;
|
|
|
+ monitor_->clamp_half_open(move_mode_);
|
|
|
+ actionType_ = eClampHalfOpen;
|
|
|
}
|
|
|
return false;
|
|
|
}
|
|
|
@@ -1193,6 +1209,8 @@ bool Navigation::clamp_open() {
|
|
|
|
|
|
bool Navigation::clamp_fully_open(){
|
|
|
if (monitor_){
|
|
|
+ if (timed_clamp_.Get() == eFullyOpened)
|
|
|
+ return true;
|
|
|
printf("Clamp fully openning...\n");
|
|
|
monitor_->clamp_fully_open(move_mode_);
|
|
|
actionType_ = eClampFullyOpen;
|
|
|
@@ -1217,13 +1235,28 @@ bool Navigation::clamp_fully_open(){
|
|
|
bool Navigation::lifter_rise() {
|
|
|
if (monitor_) {
|
|
|
printf("Lifter upping...\n");
|
|
|
- monitor_->lifter_rise(move_mode_);
|
|
|
actionType_ = eLifterRise;
|
|
|
+ if (timed_lifter_.Get() == eRose)
|
|
|
+ return true;
|
|
|
+
|
|
|
+ if (timed_lifter_.Get() != eRose){
|
|
|
+ if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
|
|
|
+ printf(" clamp statu !=eHalfOpened or eClosed, clamp_half_open...\n ");
|
|
|
+ if(!clamp_half_open()){
|
|
|
+ printf(" clamp half open failed\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
while (cancel_ == false) {
|
|
|
if (timed_lifter_.timeout()) {
|
|
|
printf("timed lifter is timeout\n");
|
|
|
return false;
|
|
|
}
|
|
|
+ if (timed_lifter_.timeout()){
|
|
|
+ return false;
|
|
|
+ }
|
|
|
if (timed_lifter_.Get() == eRose)
|
|
|
return true;
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
@@ -1238,13 +1271,26 @@ bool Navigation::lifter_rise() {
|
|
|
bool Navigation::lifter_down() {
|
|
|
if (monitor_) {
|
|
|
printf("Lifter downing...\n");
|
|
|
- monitor_->lifter_down(move_mode_);
|
|
|
+ if (timed_lifter_.Get() == eDowned)
|
|
|
+ return true;
|
|
|
actionType_ = eLifterDown;
|
|
|
+ if (timed_lifter_.Get() != eDowned){
|
|
|
+ if(timed_clamp_.Get()!=eHalfOpened && timed_clamp_.Get()!=eClosed){
|
|
|
+ printf(" clamp statu !=eHalfOpened or eClosed, clamp_half_open...\n ");
|
|
|
+ if(!clamp_half_open()){
|
|
|
+ printf(" clamp half open failed\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
while (cancel_ == false) {
|
|
|
if (timed_lifter_.timeout()) {
|
|
|
printf("timed lifter is timeout\n");
|
|
|
return false;
|
|
|
}
|
|
|
+ if (timed_lifter_.timeout()){
|
|
|
+ return false;
|
|
|
+ }
|
|
|
if (timed_lifter_.Get() == eDowned)
|
|
|
return true;
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
@@ -1543,6 +1589,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
while (newUnfinished_cations_.empty() == false && cancel_ == false) {
|
|
|
std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
|
|
|
NavMessage::NewAction act = newUnfinished_cations_.front();
|
|
|
+ NavMessage::NewAction last_act=newUnfinished_cations_.back();
|
|
|
if (act.type() == 1) { //入库
|
|
|
space_id_ = act.spacenode().id();
|
|
|
obs_w_=1.25;
|
|
|
@@ -1564,7 +1611,12 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
} else if (act.type() == 3 || act.type() == 4) { //马路导航
|
|
|
obs_w_=1.25;
|
|
|
obs_h_=1.87;
|
|
|
- if (!execute_nodes(act)){
|
|
|
+ int space_id=-1;
|
|
|
+ if(last_act.type()==1) {
|
|
|
+ std::string id = last_act.spacenode().id().substr(1, last_act.spacenode().id().length() - 1);
|
|
|
+ space_id = stoi(id);
|
|
|
+ }
|
|
|
+ if (!execute_nodes(act,space_id)){
|
|
|
printf(" street nav failed\n");
|
|
|
break;
|
|
|
}
|
|
|
@@ -1577,7 +1629,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
break;
|
|
|
}
|
|
|
} else if (act.type() == 7) {
|
|
|
- if (this->clamp_open() == false) {
|
|
|
+ if (this->clamp_fully_open() == false) {
|
|
|
printf("打开夹持 failed...\n");
|
|
|
break;
|
|
|
}
|
|
|
@@ -1623,6 +1675,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
}
|
|
|
|
|
|
void Navigation::SwitchMode(int mode, float wheelBase) {
|
|
|
+ wheelBase_=wheelBase;
|
|
|
printf("Child AGV can not Switch Mode\n");
|
|
|
}
|
|
|
|
