|
|
@@ -257,6 +257,20 @@ void Navigation::ResetLifter(LifterStatus status) {
|
|
|
|
|
|
void Navigation::ResetPose(const Pose2d &pose) {
|
|
|
timedPose_.reset(pose, 1.0);
|
|
|
+ if(pose.theta()>0){
|
|
|
+ if(parameter_.main_agv()){
|
|
|
+ isFront_.reset(true,1.0);
|
|
|
+ }else{
|
|
|
+ isFront_.reset(false,1.0);
|
|
|
+ }
|
|
|
+ }else{
|
|
|
+ if(parameter_.main_agv()){
|
|
|
+ isFront_.reset(false,1.0);
|
|
|
+ }else{
|
|
|
+ isFront_.reset(true,1.0);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
}
|
|
|
|
|
|
void Navigation::Cancel(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
|
|
|
@@ -322,7 +336,8 @@ Navigation::Navigation() {
|
|
|
move_mode_ = eSingle;
|
|
|
monitor_ = nullptr;
|
|
|
terminator_ = nullptr;
|
|
|
-
|
|
|
+ obs_h_=4.0;
|
|
|
+ obs_w_=2.0;
|
|
|
timedBrotherPose_.reset(Pose2d(-100, 0, 0), 0.5);
|
|
|
}
|
|
|
|
|
|
@@ -428,14 +443,14 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
//发送暂停消息
|
|
|
continue;
|
|
|
}
|
|
|
-// if (PoseTimeout()) {
|
|
|
-// printf(" MPC_rotate Error:Pose is timeout \n");
|
|
|
-// return eMpcFailed;
|
|
|
-// }
|
|
|
-// if (timedA_.timeout()) {
|
|
|
-// printf(" MPC_rotate Error:v/a is timeout \n");
|
|
|
-// return eMpcFailed;
|
|
|
-// }
|
|
|
+ if (PoseTimeout()) {
|
|
|
+ printf(" MPC_rotate Error:Pose is timeout \n");
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
+ if (timedA_.timeout()) {
|
|
|
+ printf(" MPC_rotate Error:v/a is timeout \n");
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
|
|
|
Pose2d current = timedPose_.Get();
|
|
|
Pose2d target(node.x(), node.y(), node.theta());
|
|
|
@@ -466,7 +481,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
if (fabs(yaw_diffs[i]) < fabs(target_yaw_diff))
|
|
|
target_yaw_diff = yaw_diffs[i];
|
|
|
}
|
|
|
- std::cout << std::endl;
|
|
|
+// std::cout << std::endl;
|
|
|
// std::cout << std::endl << " min_diff: " << current_to_target.theta() << std::endl;
|
|
|
}
|
|
|
|
|
|
@@ -482,15 +497,30 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(target_yaw_diff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(target_yaw_diff) < 0.2 * 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]);
|
|
|
+ const int down_count = 3;
|
|
|
+ double v[down_count] = {0,0,0};
|
|
|
+ double w[down_count] = {out[0], out[1], out[2]};
|
|
|
+// if(out[0]>=0){
|
|
|
+// w[0]=1.*M_PI/180.0;
|
|
|
+// w[1]=1.*M_PI/180.0;
|
|
|
+// w[2]=1.*M_PI/180.0;
|
|
|
+// }else{
|
|
|
+// w[0]=-1.*M_PI/180.0;
|
|
|
+// w[1]=-1.*M_PI/180.0;
|
|
|
+// w[2]=-1.*M_PI/180.0;
|
|
|
+// }
|
|
|
+ SendMoveCmd(move_mode_, eRotation, v, w);
|
|
|
actionType_ = eRotation;
|
|
|
- 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);
|
|
|
+ double input_w = timedA_.Get();
|
|
|
+ for (int i = 0; i < down_count; ++i) {
|
|
|
+ printf(" MPC_rotate |P[%d], input anguar:%f, down:%f(%f), diff:%f anyDirect:%d\n",
|
|
|
+ i, input_w, w[i], w[i]/M_PI*180, target_yaw_diff, directions);
|
|
|
+ }
|
|
|
}
|
|
|
continue;
|
|
|
|
|
|
@@ -511,6 +541,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
|
|
|
bool Navigation::MoveToTarget(NavMessage::PathNode node, Navigation::stLimit limit_v, Navigation::stLimit limit_w,
|
|
|
bool anyDirect, bool enable_rotate) {
|
|
|
+// LogWriter("MoveToTarget beg");
|
|
|
if (IsArrived(node)) {
|
|
|
return true;
|
|
|
}
|
|
|
@@ -581,7 +612,7 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
bool anyDirect = (action.type() == 3);
|
|
|
|
|
|
//速度限制
|
|
|
- stLimit adjust_angular = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
|
|
|
+ stLimit wmg_limit = {parameter_.nodeangularlimit().min(), parameter_.nodeangularlimit().max()};
|
|
|
stLimit mpc_velocity = {parameter_.nodevelocitylimit().min(), parameter_.nodevelocitylimit().max()};
|
|
|
|
|
|
for (int i = 0; i < action.pathnodes_size(); ++i) {
|
|
|
@@ -590,15 +621,24 @@ 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.10);
|
|
|
+ node.set_l(0.05);
|
|
|
node.set_w(0.10);
|
|
|
-
|
|
|
} else {
|
|
|
node.set_theta(0);
|
|
|
+ node.set_l(0.03);
|
|
|
node.set_w(0.10);
|
|
|
- node.set_l(0.10);
|
|
|
}
|
|
|
- if (MoveToTarget(node, mpc_velocity, adjust_angular, anyDirect, true) == false)
|
|
|
+ int directions = Direction::eForward;
|
|
|
+ if (anyDirect)
|
|
|
+ directions = Direction::eForward | Direction::eBackward |
|
|
|
+ Direction::eLeft | Direction::eRight;
|
|
|
+ if (i > 0){
|
|
|
+ if (RotateReferToTarget(node, wmg_limit, directions) != eMpcSuccess){
|
|
|
+ printf(" before move node RotateReferToNone Failed!!\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ if (MoveToTarget(node, mpc_velocity, wmg_limit, anyDirect, true) == false)
|
|
|
return false;
|
|
|
else
|
|
|
isInSpace_ = false;
|
|
|
@@ -609,8 +649,55 @@ bool Navigation::execute_nodes(NavMessage::NewAction action) {
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
+Navigation::MpcResult Navigation::RotateAfterOutSpace(){
|
|
|
+ if(move_mode_!=eDouble)
|
|
|
+ return eMpcSuccess;
|
|
|
+ Pose2d init = timedPose_.Get();
|
|
|
+ double diff1=fabs(M_PI/2.0-init.theta());
|
|
|
+ double diff2=fabs(-M_PI/2.0-init.theta());
|
|
|
+ double target= diff1<diff2?M_PI/2.0:-M_PI/2.0;
|
|
|
+ stLimit limit_rotate = {3 * M_PI / 180.0, 15 * M_PI / 180.0};
|
|
|
+ while (cancel_ == false) {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ double yawDiff = (target - current.theta());
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ //一次变速
|
|
|
+ std::vector<double> out;
|
|
|
+ bool ret;
|
|
|
+ TimerRecord::Execute([&, this]() {
|
|
|
+ ret = Rotation_mpc_once(Pose2d(0, 0, yawDiff), limit_rotate, out);
|
|
|
+ }, "Rotation_mpc_once");
|
|
|
+ if (ret == false) {
|
|
|
+ Stop();
|
|
|
+ return eMpcFailed;
|
|
|
+ }
|
|
|
+
|
|
|
+ //下发速度
|
|
|
+ if (fabs(yawDiff) < 0.3 * 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{
|
|
|
+ const int down_count = 3;
|
|
|
+ double v[down_count] = {0,0,0};
|
|
|
+ double w[down_count] = {out[0], out[1], out[2]};
|
|
|
+ SendMoveCmd(move_mode_, eRotation, v, w);
|
|
|
+ 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;
|
|
|
+
|
|
|
+ }
|
|
|
+ return eMpcFailed;
|
|
|
+}
|
|
|
+
|
|
|
Navigation::MpcResult
|
|
|
-Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
|
|
|
+Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space,
|
|
|
+ double wheelbase, NavMessage::PathNode &target) {
|
|
|
// if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
|
|
|
if (timedPose_.timeout()) {
|
|
|
printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
|
|
|
@@ -622,27 +709,35 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
double acc_angular = 20 * M_PI / 180.0;
|
|
|
double dt = 0.1;
|
|
|
Pose2d rotated = 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());
|
|
|
+ bool isFront=isFront_.Get();
|
|
|
+ printf(" front__:%d theta:%f\n",isFront,rotated.theta());
|
|
|
+ if(rotated.theta()<0.)
|
|
|
+ isFront=!isFront;
|
|
|
+
|
|
|
//前车先到,后车进入2点,保持与前车一致的朝向
|
|
|
- if (brother.in_space() && brother.space_id() == space.id()) {
|
|
|
- printf("RotateBeforeEnterSpace | 前车先到, __LINE__ = %d\n", __LINE__);
|
|
|
- rotated.mutable_theta() = brother.odom().theta();
|
|
|
+ if (isFront==false ) {
|
|
|
+ printf("后车 RotateBeforeEnterSpace | , __LINE__ = %d\n", __LINE__);
|
|
|
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);
|
|
|
+ printf("前车RotateBeforeEnterSpace | __LINE__ = %d\n", __LINE__);
|
|
|
+ //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);
|
|
|
+
|
|
|
target.set_x(x);
|
|
|
target.set_y(y);
|
|
|
target.set_theta(rotated.theta());
|
|
|
- target.set_l(0.05);
|
|
|
+ target.set_l(0.02);
|
|
|
target.set_w(0.05);
|
|
|
target.set_id(space.id());
|
|
|
printf("RotateBeforeEnterSpace | target:[x:%f,y:%f,theta:%f]\n",x,y,target.theta());
|
|
|
@@ -673,12 +768,15 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
}
|
|
|
|
|
|
//下发速度
|
|
|
- if (fabs(yawDiff) < 0.5 * M_PI / 180.0 && fabs(timedA_.Get()) < 5 * M_PI / 180.0) {
|
|
|
+ if (fabs(yawDiff) < 0.3 * 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]);
|
|
|
+ const int down_count = 3;
|
|
|
+ double v[down_count] = {0,0,0};
|
|
|
+ double w[down_count] = {out[0], out[1], out[2]};
|
|
|
+ SendMoveCmd(move_mode_, eRotation, v, w);
|
|
|
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);
|
|
|
@@ -691,6 +789,8 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
}
|
|
|
|
|
|
bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
+// LogWriter("execute_InOut_space beg");
|
|
|
+
|
|
|
if (action.type() != 1 && action.type() != 2) {
|
|
|
printf(" Inout_space failed : msg action type must ==1\n ");
|
|
|
return false;
|
|
|
@@ -709,11 +809,16 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
stLimit limit_w = {2 * M_PI / 180.0, 20 * M_PI / 180.0};
|
|
|
//入库,起点为streetNode
|
|
|
if (action.type() == 1) {
|
|
|
+// LogWriter("In space beg-");
|
|
|
+
|
|
|
Pose2d vec(action.spacenode().x() - action.streetnode().x(), action.spacenode().y() - action.streetnode().y(),
|
|
|
0);
|
|
|
- action.mutable_streetnode()->set_l(0.06);
|
|
|
- action.mutable_streetnode()->set_w(0.06);
|
|
|
+ action.mutable_streetnode()->set_l(0.02);
|
|
|
+ action.mutable_streetnode()->set_w(0.05);
|
|
|
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;
|
|
|
@@ -728,78 +833,65 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
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;
|
|
|
+ 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 (space_id == 1101){
|
|
|
+ //单车进入口搬车，夹持杆打开到全开位
|
|
|
+
|
|
|
+ }
|
|
|
//入库
|
|
|
+ Pose2d rotated = timedPose_.Get();
|
|
|
+ double x = new_target.x();
|
|
|
+ double y = new_target.y();
|
|
|
+ Pose2d vecSpace(x - rotated.x(), y - rotated.y(), 0);
|
|
|
+ rotated.mutable_theta() = Pose2d::vector2yaw(vecSpace.x(), vecSpace.y());
|
|
|
+ bool isFront=isFront_.Get();
|
|
|
+ if(rotated.theta()<0.)
|
|
|
+ isFront=!isFront;
|
|
|
+ if(isFront==false && move_mode_==eSingle){
|
|
|
+ while(timedBrotherNavStatu_.Get().in_space()==false){
|
|
|
+ if(cancel_){
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ printf(" 后车 等待 前车先入库。。。\n");
|
|
|
+ usleep(1000*500);
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ isInSpace_=true;
|
|
|
printf("Enter space beg...\n");
|
|
|
if (MoveToTarget(new_target, limit_v, limit_w, true, false) == false) {
|
|
|
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.x() < 0.05 || diff.y() < 0.05) {
|
|
|
+// LogWriter("Out space beg-");
|
|
|
+ // 计算当前位置，在车位点到马路点的连线方向上，与车位点的相对位姿
|
|
|
+ Pose2d vec(action.streetnode().x() - action.spacenode().x(),
|
|
|
+ action.streetnode().y() - action.spacenode().y(), 0);
|
|
|
+ float theta = Pose2d::vector2yaw(vec.x(), vec.y());
|
|
|
+ Pose2d space(action.spacenode().x(), action.spacenode().y(), theta);
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ Pose2d diff = Pose2d::abs(Pose2d::relativePose(Pose2d(current.x(),current.y(),0),space));
|
|
|
+ if (diff.x() < 0.05 || diff.y() < 0.10) {
|
|
|
//出库，移动到马路点，允许自由方向，不允许中途旋转
|
|
|
- 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);
|
|
|
+ action.mutable_streetnode()->set_theta(theta);
|
|
|
+ action.mutable_streetnode()->set_l(0.04);
|
|
|
+ action.mutable_streetnode()->set_w(0.06);
|
|
|
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());
|
|
|
@@ -810,12 +902,17 @@ bool Navigation::execute_InOut_space(NavMessage::NewAction action) {
|
|
|
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;
|
|
|
return false;
|
|
|
}
|
|
|
+ isInSpace_=false;
|
|
|
|
|
|
}
|
|
|
return true;
|
|
|
@@ -864,15 +961,20 @@ 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);
|
|
|
+ out.push_back(0);
|
|
|
+ out.push_back(0);
|
|
|
} 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 {}
|
|
|
+ double min_angular_velocity = 0.01;
|
|
|
+ const int down_count = 3;
|
|
|
+ for (int i = 0; i < down_count; ++i) {
|
|
|
+ if (fabs(out[i]) < min_angular_velocity) {
|
|
|
+ if (out[i] > 0)
|
|
|
+ out[i] = min_angular_velocity;
|
|
|
+ else if (out[i] < 0)
|
|
|
+ out[i] = -min_angular_velocity;
|
|
|
+ else {}
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
return ret == success || ret == no_solution;
|
|
|
@@ -896,6 +998,9 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
double velocity = timedV_.Get(); //从plc获取状态
|
|
|
double angular = timedA_.Get();
|
|
|
|
|
|
+// float diffYaw1=timedPose_.Get().m_diffYaw1;
|
|
|
+// float diffYaw2=timedPose_.Get().m_diffYaw2;
|
|
|
+
|
|
|
Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
|
|
|
|
|
|
statu[0] = pose.x();
|
|
|
@@ -907,8 +1012,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
Trajectory optimize_trajectory;
|
|
|
Trajectory selected_trajectory;
|
|
|
NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
|
|
|
- double obs_w = 0.95;//0.85;
|
|
|
- double obs_h = 1.55;//1.45;
|
|
|
+ double obs_w =obs_w_;// 0.95;//0.85;
|
|
|
+ double obs_h =obs_h_;// 1.55;//1.45;
|
|
|
|
|
|
if (directY == true) {
|
|
|
//将车身朝向旋转90°，车身朝向在(-pi,pi]
|
|
|
@@ -916,8 +1021,8 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
if (statu[2] > M_PI)
|
|
|
statu[2] -= 2 * M_PI;
|
|
|
mpc_parameter = parameter_.y_mpc_parameter();
|
|
|
- obs_w = 0.95;//0.85;
|
|
|
- obs_h = 1.55;//1.45;
|
|
|
+ obs_w = obs_h_;// 0.95;//0.85;
|
|
|
+ obs_h = obs_w_;//1.55;//1.45;
|
|
|
}
|
|
|
|
|
|
Pose2d brother = timedBrotherPose_.Get();
|
|
|
@@ -925,7 +1030,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
if (directY)
|
|
|
relative = Pose2d::relativePose(brother.rotate(brother.x(), brother.y(), M_PI / 2),
|
|
|
pose.rotate(pose.x(), pose.y(), M_PI / 2));//计算另一节在当前小车坐标系下的坐标
|
|
|
- //std::cout<<"pose:"<<pose<<", brother:"<<brother<<", relative:"<<relative<<std::endl;
|
|
|
+ std::cout<<"pose:"<<pose<<", brother:"<<brother<<", relative:"<<relative<<std::endl;
|
|
|
if (move_mode_ == eDouble)
|
|
|
relative = Pose2d(-1e8, -1e8, 0);
|
|
|
|
|
|
@@ -936,7 +1041,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
//printf(" r:%f dt:%f acc:%f acc_a:%f\n",mpc_parameter.shortest_radius(),
|
|
|
// mpc_parameter.dt(),mpc_parameter.acc_velocity(),mpc_parameter.acc_angular());
|
|
|
ret = MPC.solve(traj, traj[traj.size() - 1], statu, parameter,
|
|
|
- out, selected_trajectory, optimize_trajectory);
|
|
|
+ out, selected_trajectory, optimize_trajectory,directY);
|
|
|
|
|
|
//std::cout<<" traj size %d %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
|
|
|
if (ret == no_solution) {
|
|
|
@@ -944,7 +1049,15 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
out.clear();
|
|
|
out.push_back(0);
|
|
|
out.push_back(0);
|
|
|
+ out.push_back(0);
|
|
|
+ out.push_back(0);
|
|
|
+ out.push_back(0);
|
|
|
+ out.push_back(0);
|
|
|
}
|
|
|
+ //
|
|
|
+// diff_yaw.push_back(0);
|
|
|
+// diff_yaw.push_back(0);
|
|
|
+
|
|
|
predict_traj_.reset(optimize_trajectory, 1);
|
|
|
selected_traj_.reset(selected_trajectory, 1);
|
|
|
return ret == success || ret == no_solution;
|
|
|
@@ -998,9 +1111,10 @@ void Navigation::SendMoveCmd(int mode, ActionType type,
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-void Navigation::SendMoveCmd(int mode, ActionType type, double v, double angular, int space_id, double distance) {
|
|
|
+void Navigation::SendMoveCmd(int mode, ActionType type,
|
|
|
+ double v[], double angular[]) {
|
|
|
if (monitor_) {
|
|
|
- monitor_->set_ToAgvCmd(mode, type, v, angular, 0,space_id, distance);
|
|
|
+ monitor_->set_ToAgvCmd(mode, type, v, angular);
|
|
|
if (type == eRotation)
|
|
|
RWheel_position_ = eR;
|
|
|
if (type == eVertical)
|
|
|
@@ -1010,6 +1124,30 @@ void Navigation::SendMoveCmd(int mode, ActionType type, double v, double angular
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+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;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+//void Navigation::SendMoveCmd(int mode, ActionType type, double v[], double angular[], double yaw_diff[] ,int space_id, double distance) {
|
|
|
+// if (monitor_) {
|
|
|
+// monitor_->set_ToAgvCmd(mode, type, v, angular, yaw_diff ,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");
|
|
|
@@ -1052,6 +1190,30 @@ bool Navigation::clamp_open() {
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
+bool Navigation::clamp_fully_open(){
|
|
|
+ if (monitor_){
|
|
|
+ printf("Clamp fully openning...\n");
|
|
|
+ monitor_->clamp_fully_open(move_mode_);
|
|
|
+ actionType_ = eClampFullyOpen;
|
|
|
+ while (cancel_ == false) {
|
|
|
+ if (timed_clamp_.timeout()) {
|
|
|
+ printf("timed clamp is timeout\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if (timed_clamp_.Get() == eFullyOpened)
|
|
|
+ return true;
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ monitor_->clamp_fully_open(move_mode_);
|
|
|
+ actionType_ = eClampFullyOpen;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return false;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
bool Navigation::lifter_rise() {
|
|
|
if (monitor_) {
|
|
|
printf("Lifter upping...\n");
|
|
|
@@ -1095,6 +1257,8 @@ bool Navigation::lifter_down() {
|
|
|
}
|
|
|
|
|
|
Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit limit_v, bool autoDirect) {
|
|
|
+// LogWriter("MpcToTarget beg");
|
|
|
+
|
|
|
if (IsArrived(node)) {
|
|
|
printf(" current already in target completed !!!\n");
|
|
|
return eMpcSuccess;
|
|
|
@@ -1110,7 +1274,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
Pose2d current = timedPose_.Get();
|
|
|
Pose2d target(node.x(), node.y(), 0);
|
|
|
//生成轨迹
|
|
|
- Trajectory traj = Trajectory::create_line_trajectory(current, target);
|
|
|
+ Trajectory traj = Trajectory::create_line_trajectory(current, target, 0.1);
|
|
|
if (traj.size() == 0)
|
|
|
return eMpcSuccess;
|
|
|
//判断 巡线方向
|
|
|
@@ -1125,38 +1289,57 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
}
|
|
|
}
|
|
|
printf(" exec along autoDirect:%d\n", autoDirect);
|
|
|
+
|
|
|
+ //文件log
|
|
|
+ std::ofstream ofs; //创建流对象
|
|
|
+ std::string log_file_dir = "../data/";
|
|
|
+ time_t t = time(0);
|
|
|
+ char tmp[32]={NULL};
|
|
|
+ strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S.txt",localtime(&t));
|
|
|
+ std::string log_file_name = tmp;
|
|
|
+ ofs.open(log_file_dir+log_file_name,std::ios::app); //打开的地址和方式
|
|
|
+ auto beg_time = std::chrono::steady_clock::now();
|
|
|
+
|
|
|
while (cancel_ == false) {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
|
|
if (pause_ == true) {
|
|
|
//发送暂停消息
|
|
|
continue;
|
|
|
}
|
|
|
if (PoseTimeout()) {
|
|
|
printf(" navigation Error:Pose is timeout \n");
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
if (timedV_.timeout() || timedA_.timeout()) {
|
|
|
printf(" navigation Error:v/a is timeout | __LINE__:%d \n", __LINE__);
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
//判断是否到达终点
|
|
|
if (IsArrived(node)) {
|
|
|
if (Stop()) {
|
|
|
printf(" exec along completed !!!\n");
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eMpcSuccess;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//一次变速
|
|
|
std::vector<double> out;
|
|
|
+// std::vector<double> diff_yaw; //两车分别与整车的相对角度
|
|
|
bool ret;
|
|
|
TimerRecord::Execute([&, this]() {
|
|
|
ret = mpc_once(traj, limit_v, out, directY);
|
|
|
}, "mpc_once");
|
|
|
if (ret == false) {
|
|
|
Stop();
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
+ const int down_count = 3;//下发前多少步
|
|
|
+ double down_v[down_count] ={out[0], out[2], out[4]};//下发线速度
|
|
|
+ double down_w[down_count] ={out[1], out[3], out[5]};//下发角速度
|
|
|
/*
|
|
|
* AGV 在终点附近低速巡航时，设置最小速度0.05
|
|
|
*/
|
|
|
@@ -1165,29 +1348,35 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
target_in_agv = target_in_agv.rotate(M_PI / 2.0);
|
|
|
}
|
|
|
if (Pose2d::abs(target_in_agv) < Pose2d(0.2, 2, M_PI * 2)) {
|
|
|
- if (out[0] >= 0 && out[0] < limit_v.min)
|
|
|
- out[0] = limit_v.min;
|
|
|
- if (out[0] < 0 && out[0] > -limit_v.min)
|
|
|
- out[0] = -limit_v.min;
|
|
|
+ for (int i = 0; i < down_count; ++i) {
|
|
|
+ if (down_v[i] >= 0 && down_v[i] < limit_v.min)
|
|
|
+ down_v[i] = limit_v.min;
|
|
|
+ if (down_v[i] < 0 && down_v[i] > -limit_v.min)
|
|
|
+ down_v[i] = -limit_v.min;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//放大角速度
|
|
|
- out[1] *= 10;
|
|
|
+// out[1] *= 10;
|
|
|
|
|
|
//入库，行进方向上最后一米不纠偏
|
|
|
if (newUnfinished_cations_.front().type() == 1 && node.id().find('S')!=-1){//入库
|
|
|
- if(fabs(target_in_agv.x()) < 1.4){
|
|
|
+ if(fabs(target_in_agv.x()) < 0.5){
|
|
|
printf("target_in_agv: %f", target_in_agv.x());
|
|
|
- out[1] = 0.0;
|
|
|
+ for (double & i : down_w) {
|
|
|
+ i = 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){
|
|
|
+ if(fabs(agv_in_space.x()) < 0.3){
|
|
|
printf("agv_in_space: %f", agv_in_space.x());
|
|
|
- out[1] = 0.0;
|
|
|
+ for (double & i : down_w) {
|
|
|
+ i = 0.0;
|
|
|
+ }
|
|
|
}
|
|
|
}
|
|
|
|
|
|
@@ -1200,11 +1389,42 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
std::string id = node.id().substr(1,node.id().length()-1);
|
|
|
space_id = stoi(id);
|
|
|
}
|
|
|
+// double diff_yaw_[2] = {diff_yaw[0], diff_yaw[1]};
|
|
|
if (directY == false)
|
|
|
- SendMoveCmd(move_mode_, eVertical, out[0], out[1], space_id, distance);
|
|
|
+ SendMoveCmd(move_mode_, eVertical, down_v, down_w, space_id, distance);
|
|
|
else
|
|
|
- SendMoveCmd(move_mode_, eHorizontal, out[0], out[1], space_id, distance);
|
|
|
+ SendMoveCmd(move_mode_, eHorizontal, down_v, down_w, space_id, distance);
|
|
|
actionType_ = directY ? eHorizontal : eVertical;
|
|
|
+
|
|
|
+ //日志打印
|
|
|
+ std::string log_inf_line = std::to_string(move_mode_);
|
|
|
+ log_inf_line += " ";
|
|
|
+
|
|
|
+ log_inf_line += std::to_string(parameter_.x_mpc_parameter().shortest_radius())+" "+std::to_string(parameter_.x_mpc_parameter().acc_angular());
|
|
|
+ log_inf_line += " ";
|
|
|
+
|
|
|
+ Pose2d cur_pose = timedPose_.Get();//自身x,y,theta
|
|
|
+ log_inf_line += std::to_string(cur_pose.x()) + " " + std::to_string(cur_pose.y()) + " " + std::to_string(cur_pose.theta());
|
|
|
+ log_inf_line += " ";
|
|
|
+
|
|
|
+ Pose2d cur_Bro_pose = timedBrotherPose_.Get();//另一节x,y,theta
|
|
|
+ log_inf_line += std::to_string(cur_Bro_pose.x()) + " " + std::to_string(cur_Bro_pose.y()) + " " + std::to_string(cur_Bro_pose.theta());
|
|
|
+ log_inf_line += " ";
|
|
|
+
|
|
|
+ log_inf_line += std::to_string(out[0]) + " " + std::to_string(out[1]);//下发速度
|
|
|
+ log_inf_line += " ";
|
|
|
+ log_inf_line += std::to_string(out[2]) + " " + std::to_string(out[3]);//下发速度
|
|
|
+ log_inf_line += " ";
|
|
|
+ log_inf_line += std::to_string(out[4]) + " " + std::to_string(out[5]);//下发速度
|
|
|
+ log_inf_line += " ";
|
|
|
+
|
|
|
+ auto cur_time = std::chrono::steady_clock::now();
|
|
|
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(cur_time - beg_time);
|
|
|
+ double time =
|
|
|
+ double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
|
|
|
+ log_inf_line += std::to_string(time);
|
|
|
+
|
|
|
+ ofs << log_inf_line << std::endl; //一行日志的内容
|
|
|
/*
|
|
|
* 判断车辆是否在终点附近徘徊，无法到达终点
|
|
|
*/
|
|
|
@@ -1214,10 +1434,12 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
SlowlyStop();
|
|
|
else
|
|
|
Stop();
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eImpossibleToTarget;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+ ofs.close();//关闭文件
|
|
|
return eMpcFailed;
|
|
|
}
|
|
|
|
|
|
@@ -1284,8 +1506,11 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
|
|
|
}
|
|
|
|
|
|
|
|
|
- printf(" impossible to target:%f %f l:%f,w:%f theta:%f\n",
|
|
|
+ printf(" impossible to target:%f %f l:%f,w:%f theta:%f, ",
|
|
|
targetNode.x(), targetNode.y(), targetNode.l(), targetNode.w(), targetNode.theta());
|
|
|
+ printf(" current:%f, %f, %f\n",
|
|
|
+ current.x(), current.y(),current.theta());
|
|
|
+
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
@@ -1320,12 +1545,16 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
NavMessage::NewAction act = newUnfinished_cations_.front();
|
|
|
if (act.type() == 1) { //入库
|
|
|
space_id_ = act.spacenode().id();
|
|
|
- isInSpace_ = true;
|
|
|
+ obs_w_=1.25;
|
|
|
+ obs_h_=1.87;
|
|
|
if (!execute_InOut_space(act)) {
|
|
|
printf(" In space failed\n");
|
|
|
+ isInSpace_ = false;
|
|
|
break;
|
|
|
}
|
|
|
} else if (act.type() == 2) { //出库
|
|
|
+ obs_w_=1.25;
|
|
|
+ obs_h_=1.87;
|
|
|
if (!execute_InOut_space(act)) {
|
|
|
printf(" out space failed\n");
|
|
|
break;
|
|
|
@@ -1333,8 +1562,12 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
isInSpace_ = false;
|
|
|
}
|
|
|
} else if (act.type() == 3 || act.type() == 4) { //马路导航
|
|
|
- if (!execute_nodes(act))
|
|
|
+ obs_w_=1.25;
|
|
|
+ obs_h_=1.87;
|
|
|
+ if (!execute_nodes(act)){
|
|
|
+ printf(" street nav failed\n");
|
|
|
break;
|
|
|
+ }
|
|
|
} else if (act.type() == 5) {
|
|
|
printf(" 汽车模型导航....\n");
|
|
|
|
|
|
@@ -1402,45 +1635,46 @@ float Navigation::compute_cuv(const Pose2d &target) {
|
|
|
}
|
|
|
|
|
|
void Navigation::ManualOperation(const NavMessage::ManualCmd &cmd, NavMessage::NavResponse &response) {
|
|
|
- pause_ = false;
|
|
|
- cancel_ = false;
|
|
|
- running_ = true;
|
|
|
- actionType_ = eReady;
|
|
|
- printf("ManualOperation: %d | start...\n", cmd.operation_type());
|
|
|
-
|
|
|
- float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向
|
|
|
- double velocity;
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- switch (cmd.operation_type()) {
|
|
|
- case 1: // 旋转
|
|
|
- velocity = 2.0 * M_PI / 180.0 * symbol;
|
|
|
- SendMoveCmd(move_mode_, eRotation, 0, velocity);
|
|
|
- actionType_ = eRotation;
|
|
|
- printf(" ManualOperation: %d | input angular_v: %f, down: %f\n",
|
|
|
- cmd.operation_type(), timedA_.Get(), velocity);
|
|
|
- break;
|
|
|
- case 2: // X平移
|
|
|
- velocity = 1.0 * symbol;
|
|
|
- SendMoveCmd(move_mode_, eVertical, velocity, 0);
|
|
|
- actionType_ = eVertical;
|
|
|
- printf(" ManualOperation: %d | input line_v : %f, down: %f\n",
|
|
|
- cmd.operation_type(), timedV_.Get(), velocity);
|
|
|
- break;
|
|
|
- case 3: // Y平移
|
|
|
- velocity = 1.0 * symbol;
|
|
|
- SendMoveCmd(move_mode_, eHorizontal, velocity, 0);
|
|
|
- actionType_ = eHorizontal;
|
|
|
- printf(" ManualOperation: %d | input line_v: %f, down: %f\n",
|
|
|
- cmd.operation_type(), timedV_.Get(), velocity);
|
|
|
- break;
|
|
|
- default:
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
- actionType_ = eReady;
|
|
|
- response.set_ret(-3);
|
|
|
- response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type());
|
|
|
- printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type());
|
|
|
- running_ = false;
|
|
|
+ return;
|
|
|
+// pause_ = false;
|
|
|
+// cancel_ = false;
|
|
|
+// running_ = true;
|
|
|
+// actionType_ = eReady;
|
|
|
+// printf("ManualOperation: %d | start...\n", cmd.operation_type());
|
|
|
+//
|
|
|
+// float symbol = cmd.velocity() < 0 ? -1 : 1; // 判断为正方向or负方向
|
|
|
+// double velocity;
|
|
|
+// std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+// switch (cmd.operation_type()) {
|
|
|
+// case 1: // 旋转
|
|
|
+// velocity = 2.0 * M_PI / 180.0 * symbol;
|
|
|
+// SendMoveCmd(move_mode_, eRotation, 0, velocity);
|
|
|
+// actionType_ = eRotation;
|
|
|
+// printf(" ManualOperation: %d | input angular_v: %f, down: %f\n",
|
|
|
+// cmd.operation_type(), timedA_.Get(), velocity);
|
|
|
+// break;
|
|
|
+// case 2: // X平移
|
|
|
+// velocity = 1.0 * symbol;
|
|
|
+// SendMoveCmd(move_mode_, eVertical, velocity, 0);
|
|
|
+// actionType_ = eVertical;
|
|
|
+// printf(" ManualOperation: %d | input line_v : %f, down: %f\n",
|
|
|
+// cmd.operation_type(), timedV_.Get(), velocity);
|
|
|
+// break;
|
|
|
+// case 3: // Y平移
|
|
|
+// velocity = 1.0 * symbol;
|
|
|
+// SendMoveCmd(move_mode_, eHorizontal, velocity, 0);
|
|
|
+// actionType_ = eHorizontal;
|
|
|
+// printf(" ManualOperation: %d | input line_v: %f, down: %f\n",
|
|
|
+// cmd.operation_type(), timedV_.Get(), velocity);
|
|
|
+// break;
|
|
|
+// default:
|
|
|
+// break;
|
|
|
+// }
|
|
|
+//
|
|
|
+// actionType_ = eReady;
|
|
|
+// response.set_ret(-3);
|
|
|
+// response.set_info("ManualOperation: %d, canceled!!!", cmd.operation_type());
|
|
|
+// printf("ManualOperation: %d | canceled!!!\n", cmd.operation_type());
|
|
|
+// running_ = false;
|
|
|
}
|
|
|
|
gf