|
|
@@ -56,15 +56,16 @@ double next_speed(double speed, double target_speed, double acc, double dt) {
|
|
|
}
|
|
|
|
|
|
bool Navigation::PoseTimeout() {
|
|
|
- if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) {
|
|
|
+// if (timedPose_.timeout() == false && timedBrotherPose_.timeout() == false) {
|
|
|
+ if (timedPose_.timeout() == false) {
|
|
|
return false;
|
|
|
} else {
|
|
|
if (timedPose_.timeout()) {
|
|
|
printf(" current pose is timeout \n");
|
|
|
}
|
|
|
- if (timedBrotherPose_.timeout()) {
|
|
|
- printf(" brother pose is timeout \n");
|
|
|
- }
|
|
|
+// if (timedBrotherPose_.timeout()) {
|
|
|
+// printf(" brother pose is timeout \n");
|
|
|
+// }
|
|
|
return true;
|
|
|
}
|
|
|
}
|
|
|
@@ -99,13 +100,13 @@ bool Navigation::Init(const NavParameter::Navigation_parameter ¶meter) {
|
|
|
if (brotherEmqx_ == nullptr) {
|
|
|
brotherEmqx_ = new Terminator_emqx(brotherEmqx.nodeid());
|
|
|
|
|
|
-// if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
|
|
|
-// brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
|
|
|
-// this);
|
|
|
-// } else {
|
|
|
-// printf(" brother emqx connected failed\n");
|
|
|
-// // return false;
|
|
|
-// }
|
|
|
+ if (brotherEmqx_->Connect(brotherEmqx.ip(), brotherEmqx.port()) == true) {
|
|
|
+ brotherEmqx_->AddCallback(brotherEmqx.subbrotherstatutopic(), Navigation::BrotherAgvStatuCallback,
|
|
|
+ this);
|
|
|
+ } else {
|
|
|
+ printf(" brother emqx connected failed\n");
|
|
|
+ // return false;
|
|
|
+ }
|
|
|
//
|
|
|
}
|
|
|
}
|
|
|
@@ -137,6 +138,7 @@ void Navigation::HandleAgvStatu(const MqttMsg &msg) {
|
|
|
if (msg.toProtoMessage(speed)) {
|
|
|
ResetStatu(speed.v(), speed.w());
|
|
|
ResetClamp((ClampStatu) speed.clamp());
|
|
|
+ ResetLifter((LifterStatus) speed.lifter());
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -231,6 +233,7 @@ bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
|
|
|
plc.set_v(timedV_.Get());
|
|
|
plc.set_w(timedA_.Get());
|
|
|
plc.set_clamp(timed_clamp_.Get());
|
|
|
+ plc.set_lifter(timed_lifter_.Get());
|
|
|
//printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
|
|
|
robotStatu.mutable_agvstatu()->CopyFrom(plc);
|
|
|
}
|
|
|
@@ -248,6 +251,10 @@ void Navigation::ResetClamp(ClampStatu statu) {
|
|
|
timed_clamp_.reset(statu, 1);
|
|
|
}
|
|
|
|
|
|
+void Navigation::ResetLifter(LifterStatus status) {
|
|
|
+ timed_lifter_.reset(status, 1);
|
|
|
+}
|
|
|
+
|
|
|
void Navigation::ResetPose(const Pose2d &pose) {
|
|
|
timedPose_.reset(pose, 1.0);
|
|
|
}
|
|
|
@@ -338,7 +345,7 @@ void Navigation::BrotherAgvStatuCallback(const MqttMsg &msg, void *context) {
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-
|
|
|
+//未使用
|
|
|
bool Navigation::RotateReferToTarget(const Pose2d &target, stLimit limit_rotate, bool anyDirect) {
|
|
|
|
|
|
while (cancel_ == false) {
|
|
|
@@ -475,16 +482,29 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) {
|
|
|
+ if (fabs(target_yaw_diff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ printf(" MPC_rotate | refer target completed\\n,cuv:%f\n", target_yaw_diff);
|
|
|
+ Stop();
|
|
|
+ return eMpcSuccess;
|
|
|
+ } else{
|
|
|
SendMoveCmd(move_mode_, eRotation, 0, out[0]);
|
|
|
actionType_ = eRotation;
|
|
|
- printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n",
|
|
|
- timedA_.Get(), out[0], target_yaw_diff, directions);
|
|
|
- } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
- printf(" MPC_rotate refer target completed,cuv:%f\n", target_yaw_diff);
|
|
|
- return eMpcSuccess;
|
|
|
+ printf(" MPC_rotate | input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
|
|
|
+ timedA_.Get(), out[0], out[0]/M_PI*180, target_yaw_diff, directions);
|
|
|
}
|
|
|
continue;
|
|
|
+
|
|
|
+// //下发速度
|
|
|
+// if (fabs(target_yaw_diff) > 2 * M_PI / 180.0) {
|
|
|
+// SendMoveCmd(move_mode_, eRotation, 0, out[0]);
|
|
|
+// actionType_ = eRotation;
|
|
|
+// printf(" MPC_rotate | input angular_v:%f, down:%f, diff:%f autoDirect:%d\n",
|
|
|
+// timedA_.Get(), out[0], target_yaw_diff, directions);
|
|
|
+// } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+// printf(" MPC_rotate refer target completed,cuv:%f\n", target_yaw_diff);
|
|
|
+// return eMpcSuccess;
|
|
|
+// }
|
|
|
+// continue;
|
|
|
}
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
@@ -539,7 +559,7 @@ bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit lim
|
|
|
node.x(), node.y(), node.l(), node.w(), node.theta());
|
|
|
break;
|
|
|
} else if (ret == eImpossibleToTarget) {
|
|
|
- printf(" MPC to target:%f %f l:%f,w:%f theta:%f impossible ,retry ..............................\n",
|
|
|
+ printf(" MPC to target:%f %f l:%f,w:%f theta:%f impossible ,retry ...\n",
|
|
|
node.x(), node.y(), node.l(), node.w(), node.theta());
|
|
|
} else {
|
|
|
return false;
|
|
|
@@ -570,13 +590,13 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
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.3);
|
|
|
- node.set_w(0.1);
|
|
|
+ node.set_l(0.10);
|
|
|
+ node.set_w(0.10);
|
|
|
|
|
|
} else {
|
|
|
node.set_theta(0);
|
|
|
- node.set_w(0.2);
|
|
|
- node.set_l(0.2);
|
|
|
+ node.set_w(0.10);
|
|
|
+ node.set_l(0.10);
|
|
|
}
|
|
|
if (MoveToTarget(node, mpc_velocity, adjust_angular, anyDirect, true) == false)
|
|
|
return false;
|
|
|
@@ -591,7 +611,8 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
|
|
|
Navigation::MpcResult
|
|
|
Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
|
|
|
- if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
|
|
|
+// if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
|
|
|
+ if (timedPose_.timeout()) {
|
|
|
printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
@@ -602,39 +623,41 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
double dt = 0.1;
|
|
|
Pose2d rotated = timedPose_.Get();
|
|
|
|
|
|
- //前车先到,当前车进入2点,保持与前车一致的朝向
|
|
|
+ double x = space.x();
|
|
|
+ double y = space.y();
|
|
|
+ //前车先到,后车进入2点,保持与前车一致的朝向
|
|
|
if (brother.in_space() && brother.space_id() == space.id()) {
|
|
|
+ printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
|
|
|
rotated.mutable_theta() = brother.odom().theta();
|
|
|
- } else { //当前车先到（后车）,倒车入库
|
|
|
+ if (move_mode_ == eSingle) {
|
|
|
+ x -= wheelbase * cos(rotated.theta());
|
|
|
+ y -= wheelbase * sin(rotated.theta());
|
|
|
+ printf("确定车位点：eSingle\n");
|
|
|
+ }
|
|
|
+ } else { //当后车先到,倒车入库
|
|
|
+ printf("RotateBeforeEnterSpace | 后车先到, __LINE__ = %d\n", __LINE__);
|
|
|
rotated.mutable_theta() = space.theta();
|
|
|
rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
|
|
|
}
|
|
|
-
|
|
|
- double x = space.x();
|
|
|
- double y = space.y();
|
|
|
- if (move_mode_ == eSingle) {
|
|
|
- x -= wheelbase / 2 * cos(rotated.theta());
|
|
|
- y -= wheelbase / 2 * sin(rotated.theta());
|
|
|
- printf("确定车位点：eSingle\n");
|
|
|
- }
|
|
|
target.set_x(x);
|
|
|
target.set_y(y);
|
|
|
target.set_theta(rotated.theta());
|
|
|
target.set_l(0.05);
|
|
|
- target.set_w(0.03);
|
|
|
+ target.set_w(0.05);
|
|
|
target.set_id(space.id());
|
|
|
-
|
|
|
- //整车协调的时候，保持前后与车位一致即可
|
|
|
- if (move_mode_ == eDouble) {
|
|
|
- double yawDiff1 = (rotated - timedPose_.Get()).theta();
|
|
|
- double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
|
|
|
- if (fabs(yawDiff1) < fabs(yawDiff2)) {
|
|
|
- rotated = rotated + Pose2d(0, 0, M_PI);
|
|
|
- }
|
|
|
- }
|
|
|
+ printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
|
|
|
+
|
|
|
+// //整车协调的时候，保持前后与车位一致即可
|
|
|
+// if (move_mode_ == eDouble) {
|
|
|
+// double yawDiff1 = (rotated - timedPose_.Get()).theta();
|
|
|
+// double yawDiff2 = (rotated + Pose2d(0, 0, M_PI) - timedPose_.Get()).theta();
|
|
|
+// if (fabs(yawDiff1) < fabs(yawDiff2)) {
|
|
|
+// rotated = rotated + Pose2d(0, 0, M_PI);
|
|
|
+// }
|
|
|
+// }
|
|
|
|
|
|
while (cancel_ == false) {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
Pose2d current = timedPose_.Get();
|
|
|
double yawDiff = (rotated - current).theta();
|
|
|
|
|
|
@@ -650,36 +673,18 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(yawDiff) > 1 * M_PI / 180.0) {
|
|
|
- SendMoveCmd(move_mode_, eRotation, 0, out[0]);
|
|
|
- actionType_ = eRotation;
|
|
|
- printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f diff:%f anyDirect:false\n",
|
|
|
- timedA_.Get(), out[0], yawDiff);
|
|
|
- } else if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
|
|
|
+ Stop();
|
|
|
return eMpcSuccess;
|
|
|
+ } else{
|
|
|
+ SendMoveCmd(move_mode_, eRotation, 0, out[0]);
|
|
|
+ actionType_ = eRotation;
|
|
|
+ printf(" RotateBeforeEnterSpace | input anguar:%f, down:%f(%f), diff:%f anyDirect:false\n",
|
|
|
+ timedA_.Get(), out[0], out[0]/M_PI*180, yawDiff);
|
|
|
}
|
|
|
continue;
|
|
|
|
|
|
-
|
|
|
-// if (fabs(yawDiff) > 1 * M_PI / 180.0) {
|
|
|
-// double theta = limit_gause(yawDiff, limit_rotate.min, limit_rotate.max);
|
|
|
-// double angular = next_speed(timedA_.Get(), theta, acc_angular, dt);
|
|
|
-// double limit_angular = limit(angular, limit_rotate.min, limit_rotate.max);
|
|
|
-//
|
|
|
-// SendMoveCmd(move_mode_, Monitor_emqx::eRotation, 0, limit_angular);
|
|
|
-// actionType_ = eRotation;
|
|
|
-// printf(" RotateBeforeEnterSpace | input anguar:%f,next angular:%f,down:%f diff:%f anyDirect:false\n",
|
|
|
-// timedA_.Get(), angular, limit_angular, yawDiff);
|
|
|
-//
|
|
|
-// continue;
|
|
|
-// } else {
|
|
|
-// if (fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
-// printf(" RotateBeforeEnterSpace refer target completed\n");
|
|
|
-// printf("---------------- update new target :%f %f %f \n", target.x(), target.y(), target.theta());
|
|
|
-// return true;
|
|
|
-// }
|
|
|
-// }
|
|
|
}
|
|
|
return eMpcFailed;
|
|
|
|
|
|
@@ -706,8 +711,8 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
if (action.type() == 1) {
|
|
|
Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
|
|
|
0);
|
|
|
- action.mutable_streetnode()->set_l(0.2);
|
|
|
- action.mutable_streetnode()->set_w(0.05);
|
|
|
+ action.mutable_streetnode()->set_l(0.06);
|
|
|
+ action.mutable_streetnode()->set_w(0.06);
|
|
|
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());
|
|
|
@@ -722,24 +727,90 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
if (RotateBeforeEnterSpace(action.spacenode(), action.wheelbase(), new_target) == eMpcFailed) {
|
|
|
return false;
|
|
|
}
|
|
|
+
|
|
|
+ //进库前提升机构上升
|
|
|
+ if (lifter_rise() == false){
|
|
|
+ printf(" Enter space | lifter rise failed. line:%d\n",__LINE__);
|
|
|
+ }
|
|
|
+
|
|
|
+ //移动到预入库点位，较高精度
|
|
|
+ NavMessage::PathNode pre_target;
|
|
|
+ bool jump_preenter = false;
|
|
|
+ if (move_mode_ == eSingle){
|
|
|
+ double x = action.spacenode().x();
|
|
|
+ double y = action.spacenode().y();
|
|
|
+ double theta = action.spacenode().theta();
|
|
|
+
|
|
|
+ double back_distance = 3.3 + 0.2 + 1;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ double diff = Pose2d::distance(current, Pose2d(x,y,theta));
|
|
|
+ if (diff < back_distance) jump_preenter = true;
|
|
|
+ x -= back_distance* cos(theta);
|
|
|
+ y -= back_distance* sin(theta);
|
|
|
+
|
|
|
+ pre_target.set_x(x);
|
|
|
+ pre_target.set_y(y);
|
|
|
+ pre_target.set_theta(theta);
|
|
|
+ pre_target.set_l(0.05);
|
|
|
+ pre_target.set_w(0.05);
|
|
|
+ pre_target.set_id("R_0");
|
|
|
+ }
|
|
|
+ else if (move_mode_ == eDouble){
|
|
|
+ double x = action.spacenode().x();
|
|
|
+ double y = action.spacenode().y();
|
|
|
+ double theta = action.spacenode().theta();
|
|
|
+
|
|
|
+ double back_distance = 3.3 + 0.2 + 1 + action.wheelbase()/2;//距载车板靠墙侧车轮挡板距离（距离车位点距离）
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ double diff = Pose2d::distance(current, Pose2d(x,y,theta));
|
|
|
+ if (diff < back_distance) jump_preenter = true;
|
|
|
+ x -= back_distance* cos(theta);
|
|
|
+ y -= back_distance* sin(theta);
|
|
|
+
|
|
|
+ pre_target.set_x(x);
|
|
|
+ pre_target.set_y(y);
|
|
|
+ pre_target.set_theta(theta);
|
|
|
+ pre_target.set_l(0.05);
|
|
|
+ pre_target.set_w(0.05);
|
|
|
+ pre_target.set_id("R_0");
|
|
|
+ }
|
|
|
+ else{ return printf("PreEnter space failed. move_mode_ error!. line: %d\n", __LINE__);}
|
|
|
+ if (!jump_preenter) {
|
|
|
+ printf("PreEnter space beg...\npre_target: x:%f, y:%f, theta:%f\n",pre_target.x(),pre_target.y(),pre_target.theta());
|
|
|
+ if (MoveToTarget(pre_target, limit_v, limit_w, true, false) == false) {
|
|
|
+ printf(" PreEnter pre_space:%s failed. type:%d. line:%d\n", pre_target.id().data(), action.type(),
|
|
|
+ __LINE__);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ //入库
|
|
|
+ printf("Enter space beg...\n");
|
|
|
if (MoveToTarget(new_target, limit_v, limit_w, true, false) == false) {
|
|
|
- printf(" Enter space failed type:%d: MoveTo StreetNode failed\n", action.type());
|
|
|
+ printf(" Enter space:%s failed. type:%d\n",action.spacenode().id().data(), action.type());
|
|
|
return false;
|
|
|
}
|
|
|
} else if (action.type() == 2) {
|
|
|
Pose2d space(action.spacenode().x(), action.spacenode().y(), 0);
|
|
|
Pose2d diff = Pose2d::abs(Pose2d::relativePose(space, timedPose_.Get()));
|
|
|
- if (diff.y() < 0.05) {
|
|
|
+ if (diff.x() < 0.05 || diff.y() < 0.05) {
|
|
|
//出库，移动到马路点，允许自由方向，不允许中途旋转
|
|
|
Pose2d vec(action.streetnode().x() - action.spacenode().x(),
|
|
|
action.streetnode().y() - action.spacenode().y(), 0);
|
|
|
action.mutable_streetnode()->set_theta(Pose2d::vector2yaw(vec.x(), vec.y()));
|
|
|
action.mutable_streetnode()->set_l(0.2);
|
|
|
action.mutable_streetnode()->set_w(0.1);
|
|
|
+ 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__);
|
|
|
+ }
|
|
|
+
|
|
|
} else {
|
|
|
std::cout << " Out space failed: current pose too far from space node:" << space << ",diff:" << diff
|
|
|
<< std::endl;
|
|
|
@@ -783,7 +854,7 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
|
|
|
Pose2d brother_in_self = Pose2d::relativePose(brother, pose);
|
|
|
// std::cout<<" brother_in_self: "<<brother_in_self<<std::endl;
|
|
|
if (move_mode_ == eDouble) {
|
|
|
- brother_in_self = Pose2d(pose.x()+100,pose.y()+100,0);
|
|
|
+ brother_in_self = Pose2d(pose.x() + 100, pose.y() + 100, 0);
|
|
|
}
|
|
|
RotateMPC MPC(brother_in_self, obs_w, obs_h, limit_wmg.min, limit_wmg.max);
|
|
|
RotateMPC::MPC_parameter parameter = {mpc_parameter.shortest_radius(), mpc_parameter.dt(),
|
|
|
@@ -793,10 +864,16 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
|
|
|
printf(" Rotation_mpc solve no solution set v/w = 0\n");
|
|
|
out.clear();
|
|
|
out.push_back(0);
|
|
|
- } else
|
|
|
- {
|
|
|
- double min_angular_velocity = 3.0/180*M_PI;
|
|
|
- if (out[0] < min_angular_velocity) out[0] = min_angular_velocity;
|
|
|
+ } else {
|
|
|
+// double min_angular_velocity = 1.0 / 180 * M_PI;
|
|
|
+ double min_angular_velocity = 0.00001;
|
|
|
+ if (fabs(out[0]) < min_angular_velocity) {
|
|
|
+ if (out[0] > 0)
|
|
|
+ out[0] = min_angular_velocity;
|
|
|
+ else if (out[0] < 0)
|
|
|
+ out[0] = -min_angular_velocity;
|
|
|
+ else {}
|
|
|
+ }
|
|
|
}
|
|
|
return ret == success || ret == no_solution;
|
|
|
}
|
|
|
@@ -886,6 +963,9 @@ bool Navigation::IsArrived(NavMessage::PathNode targetNode) {
|
|
|
double l = fabs(targetNode.l());
|
|
|
double w = fabs(targetNode.w());
|
|
|
double theta = -targetNode.theta();
|
|
|
+ if (newUnfinished_cations_.front().type() == 1 && targetNode.id().find('S')!=-1){//入库只判断前后方向上是否到达
|
|
|
+ tx = x;
|
|
|
+ }
|
|
|
|
|
|
if (l < 1e-10 || w < 1e-10) {
|
|
|
printf("IsArrived: Error target l or w == 0\n");
|
|
|
@@ -918,6 +998,18 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+void Navigation::SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance) {
|
|
|
+ if (monitor_) {
|
|
|
+ monitor_->set_ToAgvCmd(mode, type, v, angular, 0,space_id, distance);
|
|
|
+ if (type == eRotation)
|
|
|
+ RWheel_position_ = eR;
|
|
|
+ if (type == eVertical)
|
|
|
+ RWheel_position_ = eX;
|
|
|
+ if (type == eHorizontal)
|
|
|
+ RWheel_position_ = eY;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
bool Navigation::clamp_close() {
|
|
|
if (monitor_) {
|
|
|
printf("Clamp closing...\n");
|
|
|
@@ -960,6 +1052,48 @@ bool Navigation::clamp_open() {
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
+bool Navigation::lifter_rise() {
|
|
|
+ if (monitor_) {
|
|
|
+ printf("Lifter upping...\n");
|
|
|
+ monitor_->lifter_rise(move_mode_);
|
|
|
+ actionType_ = eLifterRise;
|
|
|
+ while (cancel_ == false) {
|
|
|
+ if (timed_lifter_.timeout()) {
|
|
|
+ printf("timed lifter is timeout\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if (timed_lifter_.Get() == eRose)
|
|
|
+ return true;
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ monitor_->lifter_rise(move_mode_);
|
|
|
+ actionType_ = eLifterRise;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
+bool Navigation::lifter_down() {
|
|
|
+ if (monitor_) {
|
|
|
+ printf("Lifter downing...\n");
|
|
|
+ monitor_->lifter_down(move_mode_);
|
|
|
+ actionType_ = eLifterDown;
|
|
|
+ while (cancel_ == false) {
|
|
|
+ if (timed_lifter_.timeout()) {
|
|
|
+ printf("timed lifter is timeout\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if (timed_lifter_.Get() == eDowned)
|
|
|
+ return true;
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ monitor_->lifter_down(move_mode_);
|
|
|
+ actionType_ = eLifterDown;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+}
|
|
|
+
|
|
|
Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
|
|
|
if (IsArrived(node)) {
|
|
|
printf(" current already in target completed !!!\n");
|
|
|
@@ -1036,12 +1170,40 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
if (out[0] < 0 && out[0] > -limit_v.min)
|
|
|
out[0] = -limit_v.min;
|
|
|
}
|
|
|
+
|
|
|
+ //放大角速度
|
|
|
+ out[1] *= 10;
|
|
|
+
|
|
|
+ //入库，行进方向上最后一米不纠偏
|
|
|
+ if (newUnfinished_cations_.front().type() == 1 && node.id().find('S')!=-1){//入库
|
|
|
+ if(fabs(target_in_agv.x()) < 1.4){
|
|
|
+ printf("target_in_agv: %f", target_in_agv.x());
|
|
|
+ out[1] = 0.0;
|
|
|
+ }
|
|
|
+ }else if (newUnfinished_cations_.front().type() == 2){//出库,行进方向上开始一米不纠偏
|
|
|
+ Pose2d agv_in_space =
|
|
|
+ Pose2d::relativePose(
|
|
|
+ Pose2d(newUnfinished_cations_.front().spacenode().x(), newUnfinished_cations_.front().spacenode().y(), 0),
|
|
|
+ timedPose_.Get());
|
|
|
+ if(fabs(agv_in_space.x()) < 1.3){
|
|
|
+ printf("agv_in_space: %f", agv_in_space.x());
|
|
|
+ out[1] = 0.0;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
//下发速度
|
|
|
//printf(" nav input :%f out:%f\n",timedV_.Get(),out[0]);
|
|
|
+ //添加车位号,距目标点距离信息
|
|
|
+ double distance = Pose2d::distance(target_in_agv, Pose2d(0,0,0));
|
|
|
+ int space_id = 0;
|
|
|
+ if(node.id().find('S') != -1){
|
|
|
+ std::string id = node.id().substr(1,node.id().length()-1);
|
|
|
+ space_id = stoi(id);
|
|
|
+ }
|
|
|
if (directY == false)
|
|
|
- SendMoveCmd(move_mode_, eVertical, out[0], out[1]);
|
|
|
+ SendMoveCmd(move_mode_, eVertical, out[0], out[1], space_id, distance);
|
|
|
else
|
|
|
- SendMoveCmd(move_mode_, eHorizontal, out[0], out[1]);
|
|
|
+ SendMoveCmd(move_mode_, eHorizontal, out[0], out[1], space_id, distance);
|
|
|
actionType_ = directY ? eHorizontal : eVertical;
|
|
|
/*
|
|
|
* 判断车辆是否在终点附近徘徊，无法到达终点
|
|
|
@@ -1064,6 +1226,10 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
|
|
|
return false;
|
|
|
}
|
|
|
Pose2d current = timedPose_.Get();
|
|
|
+ if(targetNode.id().find('S') != -1){
|
|
|
+ //车位点强制可到达
|
|
|
+ return true;
|
|
|
+ }
|
|
|
|
|
|
double tx = targetNode.x();
|
|
|
double ty = targetNode.y();
|
|
|
@@ -1071,12 +1237,18 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
|
|
|
double w = fabs(targetNode.w());
|
|
|
double theta = -targetNode.theta();
|
|
|
|
|
|
- double W = 2.45;
|
|
|
- double L = 1.3;
|
|
|
- double minYaw = 3 * M_PI / 180.0;
|
|
|
+// double W = 2.45;
|
|
|
+// double L = 1.3;
|
|
|
+// double minYaw = 3 * M_PI / 180.0;
|
|
|
+// if (move_mode_ == eDouble) {
|
|
|
+// L = 1.3 + 2.7;
|
|
|
+//
|
|
|
+// }
|
|
|
+ double W = 2.5;
|
|
|
+ double L = 1.565;
|
|
|
+ double minYaw = 5 * M_PI / 180.0;
|
|
|
if (move_mode_ == eDouble) {
|
|
|
- L = 1.3 + 2.7;
|
|
|
-
|
|
|
+ L = 1.565 + 2.78;
|
|
|
}
|
|
|
if (directY) {
|
|
|
current = current.rotate(current.x(), current.y(), M_PI / 2);
|
|
|
@@ -1141,7 +1313,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
cancel_ = false;
|
|
|
running_ = true;
|
|
|
actionType_ = eReady;
|
|
|
- printf(" navigation beg...\n");
|
|
|
+ printf("Navigation beg...\n");
|
|
|
|
|
|
while (newUnfinished_cations_.empty() == false && cancel_ == false) {
|
|
|
std::cout << "unfinished size:" << newUnfinished_cations_.size() << std::endl;
|
|
|
@@ -1161,7 +1333,6 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
isInSpace_ = false;
|
|
|
}
|
|
|
} else if (act.type() == 3 || act.type() == 4) { //马路导航
|
|
|
-
|
|
|
if (!execute_nodes(act))
|
|
|
break;
|
|
|
} else if (act.type() == 5) {
|
|
|
@@ -1179,9 +1350,18 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
}
|
|
|
} else if (act.type() == 8) { //切换模式
|
|
|
SwitchMode(act.changedmode(), act.wheelbase());
|
|
|
- } else {
|
|
|
- printf(" action type invalid not handled !!\n");
|
|
|
- break;
|
|
|
+ } else if (act.type() == 9) { //提升机构提升
|
|
|
+ if (this->lifter_rise() == false) {
|
|
|
+ printf("提升failed ...\n");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ } else if (act.type() == 10) { //提升机构下降
|
|
|
+ if (this->lifter_down() == false) {
|
|
|
+ printf("下降failed ...\n");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }else {
|
|
|
+ printf(" action type invalid not handled !!\n");break;
|
|
|
}
|
|
|
newUnfinished_cations_.pop();
|
|
|
}
|
gf