|
|
@@ -99,14 +99,14 @@ 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;
|
|
|
+// }
|
|
|
+//
|
|
|
}
|
|
|
}
|
|
|
inited_ = true;
|
|
|
@@ -200,7 +200,7 @@ void Navigation::publish_statu(NavMessage::NavStatu &statu) {
|
|
|
pose->set_theta(pt.theta());
|
|
|
}
|
|
|
}
|
|
|
- //std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
|
|
|
+// std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
|
|
|
|
|
|
MqttMsg msg;
|
|
|
msg.fromProtoMessage(statu);
|
|
|
@@ -416,7 +416,7 @@ Navigation::MpcResult Navigation::RotateReferToTarget(NavMessage::PathNode node,
|
|
|
printf(" exec MPC_rotate autoDirect:%d\n", directions);
|
|
|
|
|
|
while (cancel_ == false) {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
if (pause_ == true) {
|
|
|
//发送暂停消息
|
|
|
continue;
|
|
|
@@ -609,8 +609,14 @@ Navigation::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase,
|
|
|
rotated.mutable_theta() = space.theta();
|
|
|
rotated = rotated.rotate(rotated.x(), rotated.y(), M_PI);
|
|
|
}
|
|
|
- double x = space.x() - wheelbase / 2 * cos(rotated.theta());
|
|
|
- double y = space.y() - wheelbase / 2 * sin(rotated.theta());
|
|
|
+
|
|
|
+ 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());
|
|
|
@@ -769,13 +775,16 @@ bool Navigation::Rotation_mpc_once(Pose2d current_to_target, Navigation::stLimit
|
|
|
statu[4] = angular_velocity;
|
|
|
|
|
|
NavParameter::MPC_parameter mpc_parameter = parameter_.x_mpc_parameter();
|
|
|
- double obs_w = 2;//0.85;
|
|
|
- double obs_h = 3;//1.45;
|
|
|
+ double obs_w = 0.95;//0.85;
|
|
|
+ double obs_h = 1.55;//1.45;
|
|
|
|
|
|
MpcError ret = failed;
|
|
|
Pose2d brother = timedBrotherPose_.Get();
|
|
|
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);
|
|
|
+ }
|
|
|
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(),
|
|
|
mpc_parameter.acc_velocity(), mpc_parameter.acc_angular()};
|
|
|
@@ -784,6 +793,10 @@ 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;
|
|
|
}
|
|
|
return ret == success || ret == no_solution;
|
|
|
}
|
|
|
@@ -817,8 +830,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.85;
|
|
|
- double obs_h = 1.45;
|
|
|
+ double obs_w = 0.95;//0.85;
|
|
|
+ double obs_h = 1.55;//1.45;
|
|
|
|
|
|
if (directY == true) {
|
|
|
//将车身朝向旋转90°，车身朝向在(-pi,pi]
|
|
|
@@ -826,8 +839,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 = 1.45;
|
|
|
- obs_h = 0.85;
|
|
|
+ obs_w = 0.95;//0.85;
|
|
|
+ obs_h = 1.55;//1.45;
|
|
|
}
|
|
|
|
|
|
Pose2d brother = timedBrotherPose_.Get();
|
|
|
@@ -836,7 +849,7 @@ bool Navigation::mpc_once(Trajectory traj, stLimit limit_v, std::vector<double>
|
|
|
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;
|
|
|
- if (move_mode_ == ActionMode::eDouble)
|
|
|
+ if (move_mode_ == eDouble)
|
|
|
relative = Pose2d(-1e8, -1e8, 0);
|
|
|
|
|
|
MpcError ret = failed;
|
|
|
@@ -979,7 +992,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
}
|
|
|
printf(" exec along autoDirect:%d\n", autoDirect);
|
|
|
while (cancel_ == false) {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(30));
|
|
|
if (pause_ == true) {
|
|
|
//发送暂停消息
|
|
|
continue;
|
|
|
@@ -1024,7 +1037,7 @@ Navigation::MpcResult Navigation::MpcToTarget(NavMessage::PathNode node, stLimit
|
|
|
out[0] = -limit_v.min;
|
|
|
}
|
|
|
//下发速度
|
|
|
- printf(" nav input :%f out:%f\n",timedV_.Get(),out[0]);
|
|
|
+ //printf(" nav input :%f out:%f\n",timedV_.Get(),out[0]);
|
|
|
if (directY == false)
|
|
|
SendMoveCmd(move_mode_, eVertical, out[0], out[1]);
|
|
|
else
|
|
|
@@ -1090,12 +1103,12 @@ bool Navigation::PossibleToTarget(NavMessage::PathNode targetNode, bool directY)
|
|
|
minR,ax,ay);*/
|
|
|
return true;
|
|
|
} else if (nagY && posiY) {
|
|
|
- std::cout << "nagY && posiY" << std::endl;
|
|
|
+// std::cout << "nagY && posiY" << std::endl;
|
|
|
return true;
|
|
|
}
|
|
|
- printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f minR:%f ax:%f,ay:%f\n", directY,
|
|
|
- relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(),
|
|
|
- minR, ax, ay);
|
|
|
+// printf("directY:%d targetInPose:%f %f l:%f,w:%f theta:%f minR:%f ax:%f,ay:%f\n", directY,
|
|
|
+// relative.x(), relative.y(), targetNode.l(), targetNode.w(), targetNode.theta(),
|
|
|
+// minR, ax, ay);
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -1165,7 +1178,7 @@ void Navigation::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &r
|
|
|
break;
|
|
|
}
|
|
|
} else if (act.type() == 8) { //切换模式
|
|
|
- SwitchMode(ActionMode(act.changedmode()), act.wheelbase());
|
|
|
+ SwitchMode(act.changedmode(), act.wheelbase());
|
|
|
} else {
|
|
|
printf(" action type invalid not handled !!\n");
|
|
|
break;
|
