|
|
@@ -3,200 +3,216 @@
|
|
|
//
|
|
|
|
|
|
#include "navigation_main.h"
|
|
|
+#include "TimerRecord.h"
|
|
|
|
|
|
|
|
|
-NavigationMain::NavigationMain(){
|
|
|
- move_mode_=Monitor_emqx::eSingle;
|
|
|
- wheelBase_=0;
|
|
|
+NavigationMain::NavigationMain() {
|
|
|
+ move_mode_ = Monitor_emqx::eSingle;
|
|
|
+ wheelBase_ = 0;
|
|
|
}
|
|
|
-NavigationMain::~NavigationMain(){
|
|
|
|
|
|
-}
|
|
|
+NavigationMain::~NavigationMain() {
|
|
|
|
|
|
-void NavigationMain::ResetPose(const Pose2d& pose){
|
|
|
- if(move_mode_==Monitor_emqx::eMain) {
|
|
|
- if(timedBrotherPose_.timeout()==true)
|
|
|
- {
|
|
|
- std::cout<<"Brother pose is timeout can not set MainAGV pose"<<std::endl;
|
|
|
- return;
|
|
|
- }
|
|
|
- //Pose2d transform(-wheelBase_/2.0,0,0);
|
|
|
- //Navigation::ResetPose(pose * transform);
|
|
|
- Pose2d brother=timedBrotherPose_.Get();
|
|
|
- Pose2d diff=Pose2d::abs(Pose2d::relativePose(brother,pose));
|
|
|
+}
|
|
|
|
|
|
- if(diff.x()>3.6 || diff.x()<2.2 || diff.y()>0.3 || diff.theta()>5*M_PI/180.0)
|
|
|
- {
|
|
|
- std::cout<<" distance with two agv is too far diff: "<<diff<<std::endl;
|
|
|
- return;
|
|
|
- }
|
|
|
- Pose2d abs_diff=pose-brother;
|
|
|
- //计算两车朝向的方向
|
|
|
- float theta=Pose2d::vector2yaw(abs_diff.x(),abs_diff.y());
|
|
|
- Pose2d agv=Pose2d((pose.x()+brother.x())/2.0,(pose.y()+brother.y())/2.0,theta);
|
|
|
-
|
|
|
- Navigation::ResetPose(agv);
|
|
|
- }
|
|
|
- else
|
|
|
- Navigation::ResetPose(pose);
|
|
|
+void NavigationMain::ResetPose(const Pose2d &pose) {
|
|
|
+ if (move_mode_ == Monitor_emqx::eMain) {
|
|
|
+ if (timedBrotherPose_.timeout() == true) {
|
|
|
+ std::cout << "Brother pose is timeout can not set MainAGV pose" << std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ //Pose2d transform(-wheelBase_/2.0,0,0);
|
|
|
+ //Navigation::ResetPose(pose * transform);
|
|
|
+ Pose2d brother = timedBrotherPose_.Get();
|
|
|
+ Pose2d diff = Pose2d::abs(Pose2d::relativePose(brother, pose));
|
|
|
+
|
|
|
+ if (diff.x() > 3.6 || diff.x() < 2.2 || diff.y() > 0.3 || diff.theta() > 5 * M_PI / 180.0) {
|
|
|
+ std::cout << " distance with two agv is too far diff: " << diff << std::endl;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ Pose2d abs_diff = pose - brother;
|
|
|
+ //计算两车朝向的方向
|
|
|
+ float theta = Pose2d::vector2yaw(abs_diff.x(), abs_diff.y());
|
|
|
+ Pose2d agv = Pose2d((pose.x() + brother.x()) / 2.0, (pose.y() + brother.y()) / 2.0, theta);
|
|
|
+
|
|
|
+ Navigation::ResetPose(agv);
|
|
|
+ } else
|
|
|
+ Navigation::ResetPose(pose);
|
|
|
}
|
|
|
|
|
|
-void NavigationMain::publish_statu(NavMessage::NavStatu& statu)
|
|
|
-{
|
|
|
- statu.set_main_agv(true);
|
|
|
- Navigation::publish_statu(statu);
|
|
|
+void NavigationMain::publish_statu(NavMessage::NavStatu &statu) {
|
|
|
+ statu.set_main_agv(true);
|
|
|
+ Navigation::publish_statu(statu);
|
|
|
}
|
|
|
|
|
|
|
|
|
-void NavigationMain::Start(const NavMessage::NavCmd& cmd,NavMessage::NavResponse& response)
|
|
|
-{
|
|
|
- /*if(move_mode_!=Monitor_emqx::eMain)
|
|
|
- {
|
|
|
- printf(" navigation mode must set main,parameter:Pose2d\n");
|
|
|
- return false;
|
|
|
- }*/
|
|
|
+void NavigationMain::Start(const NavMessage::NavCmd &cmd, NavMessage::NavResponse &response) {
|
|
|
+ /*if(move_mode_!=Monitor_emqx::eMain)
|
|
|
+ {
|
|
|
+ printf(" navigation mode must set main,parameter:Pose2d\n");
|
|
|
+ return false;
|
|
|
+ }*/
|
|
|
|
|
|
- Navigation::Start(cmd,response);
|
|
|
+ Navigation::Start(cmd, response);
|
|
|
|
|
|
}
|
|
|
|
|
|
-void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode,Monitor_emqx::ActionType type,
|
|
|
- double v,double angular)
|
|
|
-{
|
|
|
- if(monitor_)
|
|
|
- {
|
|
|
- monitor_->set_speed(mode,type,v,angular,wheelBase_);
|
|
|
- }
|
|
|
+void NavigationMain::SendMoveCmd(Monitor_emqx::ActionMode mode, Monitor_emqx::ActionType type,
|
|
|
+ double v, double angular) {
|
|
|
+ if (monitor_) {
|
|
|
+ monitor_->set_speed(mode, type, v, angular, wheelBase_);
|
|
|
+ }
|
|
|
|
|
|
}
|
|
|
|
|
|
-bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
|
|
|
-{
|
|
|
- if(Navigation::CreateRobotStatuMsg(robotStatu)) {
|
|
|
- robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
|
|
|
- return true;
|
|
|
- }
|
|
|
- //std::cout<<agvStatu.DebugString()<<std::endl;
|
|
|
- return false;
|
|
|
+bool NavigationMain::CreateRobotStatuMsg(NavMessage::RobotStatu &robotStatu) {
|
|
|
+ if (Navigation::CreateRobotStatuMsg(robotStatu)) {
|
|
|
+ robotStatu.mutable_agvstatu()->set_clamp_other(timed_other_clamp_.Get());
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ //std::cout<<agvStatu.DebugString()<<std::endl;
|
|
|
+ return false;
|
|
|
}
|
|
|
|
|
|
-void NavigationMain::ResetOtherClamp(ClampStatu statu)
|
|
|
-{
|
|
|
- timed_other_clamp_.reset(statu,1);
|
|
|
+void NavigationMain::ResetOtherClamp(ClampStatu statu) {
|
|
|
+ timed_other_clamp_.reset(statu, 1);
|
|
|
}
|
|
|
|
|
|
-void NavigationMain::HandleAgvStatu(const MqttMsg& msg)
|
|
|
-{
|
|
|
- NavMessage::AgvStatu speed;
|
|
|
- if(msg.toProtoMessage(speed))
|
|
|
- {
|
|
|
- ResetStatu(speed.v(),speed.w());
|
|
|
- ResetClamp((ClampStatu)speed.clamp());
|
|
|
- ResetOtherClamp((ClampStatu)speed.clamp_other());
|
|
|
- //printf(" clamp:%d other:%d\n",speed.clamp(),speed.clamp_other());
|
|
|
- }
|
|
|
+void NavigationMain::HandleAgvStatu(const MqttMsg &msg) {
|
|
|
+ NavMessage::AgvStatu speed;
|
|
|
+ if (msg.toProtoMessage(speed)) {
|
|
|
+ ResetStatu(speed.v(), speed.w());
|
|
|
+ ResetClamp((ClampStatu) speed.clamp());
|
|
|
+ ResetOtherClamp((ClampStatu) speed.clamp_other());
|
|
|
+ //printf(" clamp:%d other:%d\n",speed.clamp(),speed.clamp_other());
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
-bool NavigationMain::clamp_close()
|
|
|
-{
|
|
|
- if(move_mode_==Monitor_emqx::eSingle)
|
|
|
- return Navigation::clamp_close();
|
|
|
- printf("双车夹持\n");
|
|
|
- if(monitor_) {
|
|
|
- monitor_->clamp_close(move_mode_);
|
|
|
- while (exit_ == false) {
|
|
|
- if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
|
|
|
- printf("timed clamp is timeout\n");
|
|
|
+bool NavigationMain::clamp_close() {
|
|
|
+ if (move_mode_ == Monitor_emqx::eSingle)
|
|
|
+ return Navigation::clamp_close();
|
|
|
+ printf("双车夹持\n");
|
|
|
+ if (monitor_) {
|
|
|
+ monitor_->clamp_close(move_mode_);
|
|
|
+ while (exit_ == false) {
|
|
|
+ if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
|
|
|
+ printf("timed clamp is timeout\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get() == eClosed) {
|
|
|
+ printf("双车夹持completed!!!\n");
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ monitor_->clamp_close(move_mode_);
|
|
|
+ }
|
|
|
return false;
|
|
|
- }
|
|
|
- if (timed_clamp_.Get() == eClosed && timed_other_clamp_.Get()==eClosed) {
|
|
|
- printf("双车夹持completed!!!\n");
|
|
|
- return true;
|
|
|
- }
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- monitor_->clamp_close(move_mode_);
|
|
|
}
|
|
|
return false;
|
|
|
- }
|
|
|
- return false;
|
|
|
}
|
|
|
-bool NavigationMain::clamp_open() {
|
|
|
- if(move_mode_==Monitor_emqx::eSingle)
|
|
|
- return Navigation::clamp_open();
|
|
|
|
|
|
- if(monitor_) {
|
|
|
- printf("双车松夹持\n");
|
|
|
- monitor_->clamp_open(move_mode_);
|
|
|
- while(exit_==false)
|
|
|
- {
|
|
|
- if(timed_clamp_.timeout()||timed_other_clamp_.timeout())
|
|
|
- {
|
|
|
- printf("timed clamp is timeout\n");
|
|
|
+bool NavigationMain::clamp_open() {
|
|
|
+ if (move_mode_ == Monitor_emqx::eSingle)
|
|
|
+ return Navigation::clamp_open();
|
|
|
+
|
|
|
+ if (monitor_) {
|
|
|
+ printf("双车松夹持\n");
|
|
|
+ monitor_->clamp_open(move_mode_);
|
|
|
+ while (exit_ == false) {
|
|
|
+ if (timed_clamp_.timeout() || timed_other_clamp_.timeout()) {
|
|
|
+ printf("timed clamp is timeout\n");
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ if (timed_clamp_.Get() == eOpened && timed_other_clamp_.Get() == eOpened) {
|
|
|
+ printf("双车松夹持completed!!!\n");
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ monitor_->clamp_open(move_mode_);
|
|
|
+ }
|
|
|
return false;
|
|
|
- }
|
|
|
- if(timed_clamp_.Get()==eOpened&&timed_other_clamp_.Get()==eOpened) {
|
|
|
- printf("双车松夹持completed!!!\n");
|
|
|
- return true;
|
|
|
- }
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- monitor_->clamp_open(move_mode_);
|
|
|
}
|
|
|
return false;
|
|
|
- }
|
|
|
- return false;
|
|
|
}
|
|
|
|
|
|
-bool NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space,double wheelbase,NavMessage::PathNode& target) {
|
|
|
- if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
|
|
|
- printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();
|
|
|
-
|
|
|
- stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
|
|
|
- double acc_angular = 25 * M_PI / 180.0;
|
|
|
- double dt = 0.1;
|
|
|
- Pose2d rotated = Pose2d(space.x(),space.y(),space.theta());
|
|
|
- target.set_l(0.05);
|
|
|
- target.set_w(0.03);
|
|
|
- target.set_id(space.id());
|
|
|
- //后车先到,当前车进入2点,保持与后车一致的朝向
|
|
|
- if (brother.in_space() && brother.space_id() == space.id()) {
|
|
|
- rotated.mutable_theta() = brother.odom().theta();
|
|
|
- } else { //当前车先到,正向
|
|
|
- rotated.mutable_theta() = space.theta();
|
|
|
- }
|
|
|
- std::cout<<"===============================> RotateBeforeEnterSpace ,target:"<<rotated<<std::endl;
|
|
|
- double x = space.x() + wheelbase/2.0 * cos(rotated.theta());
|
|
|
- double y = space.y() + wheelbase/2.0 * sin(rotated.theta());
|
|
|
- target.set_x(x);
|
|
|
- target.set_y(y);
|
|
|
- target.set_theta(rotated.theta());
|
|
|
-
|
|
|
- while (cancel_ == false) {
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
- Pose2d current = timedPose_.Get();
|
|
|
- double yawDiff = (rotated - current).theta();
|
|
|
- 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::eRotate, 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;
|
|
|
- }
|
|
|
+Navigation::MpcResult
|
|
|
+NavigationMain::RotateBeforeEnterSpace(NavMessage::PathNode space, double wheelbase, NavMessage::PathNode &target) {
|
|
|
+ if (timedBrotherNavStatu_.timeout() || timedPose_.timeout()) {
|
|
|
+ printf(" rotate failed : timedBrotherNavStatu_ or pose timeout\n");
|
|
|
+ return eMpcFailed;
|
|
|
}
|
|
|
- }
|
|
|
- return false;
|
|
|
|
|
|
+ NavMessage::NavStatu brother = timedBrotherNavStatu_.Get();
|
|
|
+
|
|
|
+ stLimit limit_rotate = {2 * M_PI / 180.0, 15 * M_PI / 180.0};
|
|
|
+ double acc_angular = 25 * M_PI / 180.0;
|
|
|
+ double dt = 0.1;
|
|
|
+ Pose2d rotated = Pose2d(space.x(), space.y(), space.theta());
|
|
|
+ target.set_l(0.05);
|
|
|
+ target.set_w(0.03);
|
|
|
+ target.set_id(space.id());
|
|
|
+ //后车先到,当前车进入2点,保持与后车一致的朝向
|
|
|
+ if (brother.in_space() && brother.space_id() == space.id()) {
|
|
|
+ rotated.mutable_theta() = brother.odom().theta();
|
|
|
+ } else { //当前车先到,正向
|
|
|
+ rotated.mutable_theta() = space.theta();
|
|
|
+ }
|
|
|
+ std::cout << "===============================> RotateBeforeEnterSpace ,target:" << rotated << std::endl;
|
|
|
+ double x = space.x() + wheelbase / 2.0 * cos(rotated.theta());
|
|
|
+ double y = space.y() + wheelbase / 2.0 * sin(rotated.theta());
|
|
|
+ target.set_x(x);
|
|
|
+ target.set_y(y);
|
|
|
+ target.set_theta(rotated.theta());
|
|
|
+
|
|
|
+ while (cancel_ == false) {
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+ Pose2d current = timedPose_.Get();
|
|
|
+ double yawDiff = (rotated - 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) > 1 * M_PI / 180.0) {
|
|
|
+ SendMoveCmd(move_mode_, Monitor_emqx::eRotate, 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) {
|
|
|
+ printf(" RotateBeforeEnterSpace refer target completed\\n,cuv:%f\n", yawDiff);
|
|
|
+ return eMpcSuccess;
|
|
|
+ }
|
|
|
+ continue;
|
|
|
+
|
|
|
+// std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
|
+// Pose2d current = timedPose_.Get();
|
|
|
+// double yawDiff = (rotated - current).theta();
|
|
|
+// 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::eRotate, 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;
|
|
|
}
|
