|
|
@@ -176,7 +176,7 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
|
|
|
if (running_)
|
|
|
{
|
|
|
statu.set_key(global_navCmd_.key());
|
|
|
- std::queue<NavMessage::Action> actios = unfinished_cations_;
|
|
|
+ /*std::queue<NavMessage::Action> actios = unfinished_cations_;
|
|
|
while (actios.empty() == false) {
|
|
|
//保存当前动作
|
|
|
NavMessage::Action *act = statu.add_unfinished_actions();
|
|
|
@@ -185,11 +185,9 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
|
|
|
act->CopyFrom(actios.front());
|
|
|
actios.pop();
|
|
|
|
|
|
- }
|
|
|
+ }*/
|
|
|
}
|
|
|
//发布MPC信息
|
|
|
- if (current_action != nullptr) {
|
|
|
- delete current_action;
|
|
|
//发布mpc 预选点
|
|
|
if (selected_traj_.timeout() == false) {
|
|
|
for (int i = 0; i < selected_traj_.Get().size(); ++i) {
|
|
|
@@ -199,6 +197,7 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
|
|
|
pose->set_y(pt.y());
|
|
|
pose->set_theta(pt.theta());
|
|
|
}
|
|
|
+
|
|
|
}
|
|
|
//发布 mpc 预测点
|
|
|
if (predict_traj_.timeout() == false) {
|
|
|
@@ -210,24 +209,26 @@ void Navigation::publish_statu(NavMessage::NavStatu& statu) {
|
|
|
pose->set_theta(pt.theta());
|
|
|
}
|
|
|
}
|
|
|
- }
|
|
|
+ //std::cout<<"nav statu:"<<statu.DebugString()<<std::endl;
|
|
|
+
|
|
|
MqttMsg msg;
|
|
|
msg.fromProtoMessage(statu);
|
|
|
if(terminator_)
|
|
|
terminator_->Publish(parameter_.terminal_emqx().pubnavstatutopic(),msg);
|
|
|
|
|
|
//发布位姿 ------robot状态--------------------------
|
|
|
- NavMessage::RobotStatu robot=CreateRobotStatuMsg();
|
|
|
- if(terminator_) {
|
|
|
+ NavMessage::RobotStatu robot;
|
|
|
+ if(CreateRobotStatuMsg(robot)) {
|
|
|
+ if (terminator_) {
|
|
|
MqttMsg msg;
|
|
|
msg.fromProtoMessage(robot);
|
|
|
terminator_->Publish(parameter_.terminal_emqx().pubstatutopic(), msg);
|
|
|
}
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
-NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
|
|
|
+bool Navigation::CreateRobotStatuMsg(NavMessage::RobotStatu& robotStatu)
|
|
|
{
|
|
|
- NavMessage::RobotStatu robotStatu;
|
|
|
//printf(" %d %d %d \n",timedPose_.timeout(),timedV_.timeout(),timedA_.timeout());
|
|
|
if (timedPose_.timeout() == false) {
|
|
|
|
|
|
@@ -243,8 +244,9 @@ NavMessage::RobotStatu Navigation::CreateRobotStatuMsg()
|
|
|
//printf(" timed_clamp_:%d\n ",timed_clamp_.Get());
|
|
|
robotStatu.mutable_agvstatu()->CopyFrom(plc);
|
|
|
}
|
|
|
+ return true;
|
|
|
}
|
|
|
- return robotStatu;
|
|
|
+ return false;
|
|
|
}
|
|
|
|
|
|
void Navigation::ResetStatu(double v,double a)
|
|
|
@@ -283,8 +285,14 @@ bool Navigation::Start(const NavMessage::NavCmd& cmd)
|
|
|
|
|
|
//add 先检查当前点与起点的距离
|
|
|
global_navCmd_=cmd;
|
|
|
- for(int i=0;i<cmd.actions_size();++i)
|
|
|
+ if(cmd.action()==6)
|
|
|
+ {
|
|
|
+ for (int i = 0; i < cmd.newactions_size(); ++i)
|
|
|
+ newUnfinished_cations_.push(cmd.newactions(i));
|
|
|
+ }else {
|
|
|
+ for (int i = 0; i < cmd.actions_size(); ++i)
|
|
|
unfinished_cations_.push(cmd.actions(i));
|
|
|
+ }
|
|
|
thread_=new std::thread(&Navigation::navigatting,this);
|
|
|
return true;
|
|
|
|
|
|
@@ -367,7 +375,7 @@ void Navigation::HandleNavCmd(const NavMessage::NavCmd& cmd)
|
|
|
Pause();
|
|
|
return ;
|
|
|
}
|
|
|
- if(cmd.action()==0)
|
|
|
+ if(cmd.action()==0||cmd.action()==6)
|
|
|
{
|
|
|
Start(cmd);
|
|
|
}
|
|
|
@@ -388,16 +396,17 @@ void Navigation::NavCmdCallback(const MqttMsg& msg,void* context)
|
|
|
|
|
|
|
|
|
|
|
|
-bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,
|
|
|
- stLimit limit_v,stLimit limit_h,stLimit limit_rotate,bool nearest_yaw)
|
|
|
+bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target_diff,stLimit limit_v,
|
|
|
+ stLimit limit_h,stLimit limit_rotate,bool anyDitect)
|
|
|
{
|
|
|
- std::cout<<" adjust tarhet:"<<target<<" diff:"<<target_diff<<std::endl;
|
|
|
+ std::cout<<" adjust target:"<<target<<" target diff:"<<target_diff<<std::endl;
|
|
|
if(inited_==false) {
|
|
|
printf(" navigation has not inited\n");
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- Pose2d thresh=Pose2d::abs(target_diff);
|
|
|
+ Pose2d accuracy=target_diff;
|
|
|
+ Pose2d thresh=Pose2d::abs(accuracy);
|
|
|
|
|
|
int action=0; //1 原地旋转，2横移，3前进
|
|
|
while(cancel_==false) {
|
|
|
@@ -411,20 +420,20 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
|
|
|
break;
|
|
|
}
|
|
|
//判断是否到达目标点
|
|
|
- if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, target_diff,nearest_yaw)) {
|
|
|
+ if (IsArrived(timedPose_.Get(), timedV_.Get(), timedA_.Get(), target, accuracy,anyDitect)) {
|
|
|
if(Stop()) {
|
|
|
- printf("adjust completed nearest yaw:%d\n",nearest_yaw);
|
|
|
+ printf("adjust completed anyDitect :%d\n",anyDitect);
|
|
|
return true;
|
|
|
}
|
|
|
return false;
|
|
|
}
|
|
|
+
|
|
|
|
|
|
//计算目标点在当前pose下的pose
|
|
|
Pose2d diff = Pose2d::relativePose(target, timedPose_.Get());
|
|
|
-
|
|
|
//停止状态
|
|
|
if (action == 0) {
|
|
|
- //优先进入旋转
|
|
|
+ //
|
|
|
if (Pose2d::abs(diff).x() > thresh.x()) {
|
|
|
//前进
|
|
|
action = 3;
|
|
|
@@ -444,7 +453,7 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
|
|
|
if (action == 1) {
|
|
|
if (Pose2d::abs(diff).theta() > thresh.theta()) {
|
|
|
float diff_yaw=diff.theta();
|
|
|
- if(nearest_yaw==true)
|
|
|
+ if(anyDitect==true)
|
|
|
{
|
|
|
int n=int(diff_yaw/(M_PI/4.0));
|
|
|
if (n%2==1)
|
|
|
@@ -457,14 +466,14 @@ bool Navigation::execute_adjust_action(const Pose2d& target,const Pose2d& target
|
|
|
diff_yaw=diff_yaw-float(n/2)*M_PI/2.0;
|
|
|
}
|
|
|
}
|
|
|
- double theta = limit_gause(diff_yaw,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
|
|
|
+ double theta = limit_gause(diff_yaw,limit_rotate.min,limit_rotate.max);
|
|
|
double angular=next_speed(timedA_.Get(),theta,acc_angular,dt);
|
|
|
- double limit_angular=limit(angular,limit_rotate.min*M_PI/180.0,limit_rotate.max*M_PI/180.0);
|
|
|
+ double limit_angular=limit(angular,limit_rotate.min,limit_rotate.max);
|
|
|
|
|
|
SendMoveCmd(move_mode_,Monitor_emqx::eRotate, 0, limit_angular);
|
|
|
actionType_=eRotation;
|
|
|
- printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f NearestYaw:%d\n",
|
|
|
- timedA_.Get(),angular,limit_angular,theta,nearest_yaw);
|
|
|
+ printf(" Ratate | input anguar:%f,next angular:%f,down:%f diff:%f anyDitect:%d\n",
|
|
|
+ timedA_.Get(),angular,limit_angular,diff_yaw,anyDitect);
|
|
|
continue;
|
|
|
}
|
|
|
}
|
|
|
@@ -509,8 +518,9 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
|
|
|
std::cout << "execute pathNodes failed ,Action invalid:" << action.DebugString() << std::endl;
|
|
|
return false;
|
|
|
}
|
|
|
- bool autoDirect=(action.type()==3);
|
|
|
- //原地调整
|
|
|
+ bool anyDirect=(action.type()==3);
|
|
|
+
|
|
|
+ //原地调整起点
|
|
|
NavMessage::PathNode first=action.pathnodes(0);
|
|
|
//速度限制
|
|
|
stLimit adjust_x={ action.adjustvelocitylimit().min(),action.adjustvelocitylimit().max() };
|
|
|
@@ -518,22 +528,40 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
|
|
|
stLimit adjust_angular={ action.nodeangularlimit().min(),action.nodeangularlimit().max() };
|
|
|
stLimit mpc_velocity={ action.nodevelocitylimit().min(),action.nodevelocitylimit().max() };
|
|
|
//目标，精度
|
|
|
- Pose2d target(first.pose().x(),first.pose().y(),first.pose().theta());
|
|
|
- Pose2d accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
|
|
|
- execute_adjust_action(target,accuracy,adjust_x,adjust_h,adjust_angular,autoDirect);
|
|
|
- NavMessage::PathNode last_node=first;
|
|
|
-
|
|
|
+ double first_yaw=timedPose_.Get().theta();
|
|
|
+ if(1<action.pathnodes_size()){
|
|
|
+ NavMessage::PathNode next=action.pathnodes(1);
|
|
|
+ first_yaw=Pose2d::vector2yaw(next.pose().x()-first.pose().x(),next.pose().y()-first.pose().y());
|
|
|
+ }
|
|
|
+ Pose2d first_target(first.pose().x(),first.pose().y(),first_yaw);
|
|
|
+ Pose2d first_accuracy(first.accuracy().x(),first.accuracy().y(),first.accuracy().theta());
|
|
|
+ execute_adjust_action(first_target,first_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
|
|
|
+ Pose2d last_node=first_target;
|
|
|
for(int i=1;i< action.pathnodes_size();++i)
|
|
|
{
|
|
|
- NavMessage::PathNode node=action.pathnodes(i);
|
|
|
- /// 巡线到目标点，判断巡线方向
|
|
|
- Pose2d begin(last_node.pose().x(),last_node.pose().y(),last_node.pose().theta());
|
|
|
- Pose2d target(node.pose().x(),node.pose().y(),node.pose().theta());
|
|
|
- Pose2d accuracy(node.accuracy().x(),node.accuracy().y(),node.accuracy().theta());
|
|
|
+ NavMessage::PathNode node = action.pathnodes(i);
|
|
|
+ /// 巡线到目标点，判断巡线方向
|
|
|
+
|
|
|
+ Pose2d target(node.pose().x(), node.pose().y(), node.pose().theta());
|
|
|
+ Pose2d accuracy(node.accuracy().x(), node.accuracy().y(), node.accuracy().theta());
|
|
|
+
|
|
|
+ if (execute_along_action(last_node, target, accuracy, mpc_velocity, anyDirect) == false)
|
|
|
+ return false;
|
|
|
+ //不是最后一个点，则原地当前点朝向
|
|
|
+ if(i!=action.pathnodes_size()-1)
|
|
|
+ {
|
|
|
+ //目标，精度
|
|
|
+ double adjust_yaw=timedPose_.Get().theta();
|
|
|
+ NavMessage::PathNode next=action.pathnodes(i+1);
|
|
|
+ adjust_yaw=Pose2d::vector2yaw(next.pose().x()-node.pose().x(),next.pose().y()-node.pose().y());
|
|
|
+
|
|
|
+ Pose2d adjust_target(node.pose().x(),node.pose().y(),adjust_yaw);
|
|
|
+ Pose2d adjust_accuracy(node.accuracy().x(),node.accuracy().y(),node.accuracy().theta());
|
|
|
+ execute_adjust_action(adjust_target,adjust_accuracy,adjust_x,adjust_h,adjust_angular,anyDirect);
|
|
|
+ last_node=adjust_target;
|
|
|
+ }
|
|
|
+
|
|
|
|
|
|
- if(execute_along_action(begin,target,accuracy,mpc_velocity,autoDirect)==false)
|
|
|
- return false;
|
|
|
- last_node=node;
|
|
|
}
|
|
|
return true;
|
|
|
}
|
|
|
@@ -541,55 +569,56 @@ bool Navigation::execute_nodes(NavMessage::NewAction action)
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
-bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY)
|
|
|
-{
|
|
|
- if(timedPose_.timeout()==true && timedV_.timeout()==false && timedA_.timeout()==false)
|
|
|
- {
|
|
|
+bool Navigation::mpc_once(Trajectory traj,stLimit limit_v,std::vector<double>& out,bool directY) {
|
|
|
+ if (timedPose_.timeout() == true && timedV_.timeout() == false && timedA_.timeout() == false) {
|
|
|
printf(" MPC once Error:Pose/V/A is timeout \n");
|
|
|
return false;
|
|
|
}
|
|
|
- if(traj.size()==0)
|
|
|
- {
|
|
|
+ if (traj.size() == 0) {
|
|
|
printf("traj size ==0\n");
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- Pose2d pose=timedPose_.Get();
|
|
|
- double velocity=timedV_.Get(); //从plc获取状态
|
|
|
- double angular=timedA_.Get();
|
|
|
+ Pose2d pose = timedPose_.Get();
|
|
|
+ double velocity = timedV_.Get(); //从plc获取状态
|
|
|
+ double angular = timedA_.Get();
|
|
|
|
|
|
- Pose2d brother=timedBrotherPose_.Get();
|
|
|
- Pose2d relative=Pose2d::relativePose(brother,pose);//计算另一节在当前小车坐标系下的坐标
|
|
|
- if(move_mode_==Monitor_emqx::ActionMode::eMain)
|
|
|
- relative=Pose2d(-1e8,-1e8,0);
|
|
|
+ Pose2d brother = timedBrotherPose_.Get();
|
|
|
+ Pose2d relative = Pose2d::relativePose(brother, pose);//计算另一节在当前小车坐标系下的坐标
|
|
|
+ if (move_mode_ == Monitor_emqx::ActionMode::eMain)
|
|
|
+ relative = Pose2d(-1e8, -1e8, 0);
|
|
|
|
|
|
- Eigen::VectorXd statu=Eigen::VectorXd::Zero(5);//agv状态
|
|
|
+ Eigen::VectorXd statu = Eigen::VectorXd::Zero(5);//agv状态
|
|
|
|
|
|
- statu[0]=pose.x();
|
|
|
- statu[1]=pose.y();
|
|
|
- statu[2]=pose.theta();
|
|
|
- statu[3]=velocity;
|
|
|
- statu[4]=angular;
|
|
|
+ statu[0] = pose.x();
|
|
|
+ statu[1] = pose.y();
|
|
|
+ statu[2] = pose.theta();
|
|
|
+ statu[3] = velocity;
|
|
|
+ statu[4] = angular;
|
|
|
|
|
|
Trajectory optimize_trajectory;
|
|
|
Trajectory selected_trajectory;
|
|
|
|
|
|
- bool ret=false;
|
|
|
- if(directY==false) {
|
|
|
- LoadedMPC MPC(relative, limit_v.min, limit_v.max);
|
|
|
- ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
|
|
|
- }else{
|
|
|
- MPC_Y MPC(relative, limit_v.min, limit_v.max);
|
|
|
- ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
|
|
|
+ if (directY == true) {
|
|
|
+ //将车身朝向旋转90°，车身朝向在(-pi,pi]
|
|
|
+ statu[2] += M_PI / 2;
|
|
|
+ if (statu[2] > M_PI)
|
|
|
+ statu[2] -= 2 * M_PI;
|
|
|
}
|
|
|
- predict_traj_.reset(optimize_trajectory,1);
|
|
|
- selected_traj_.reset(selected_trajectory,1);
|
|
|
+ bool ret = false;
|
|
|
+ LoadedMPC MPC(relative, limit_v.min, limit_v.max);
|
|
|
+ ret = MPC.solve(traj, traj[traj.size() - 1], statu, out, selected_trajectory, optimize_trajectory);
|
|
|
+
|
|
|
+ //std::cout<<" traj size %d %d -------- "<<selected_trajectory.size()<<","<<optimize_trajectory.size()<<std::endl;
|
|
|
+ predict_traj_.reset(optimize_trajectory, 1);
|
|
|
+ selected_traj_.reset(selected_trajectory, 1);
|
|
|
return ret;
|
|
|
}
|
|
|
|
|
|
bool Navigation::IsArrived(const Pose2d& cur,double velocity,double angular,
|
|
|
const Pose2d& target,const Pose2d& diff,bool nearest_yaw)
|
|
|
{
|
|
|
+ //std::cout<<cur<<","<<target<<","<<nearest_yaw<<",v"<<velocity<<"a:"<<angular<<"diff:"<<diff<<std::endl;
|
|
|
Pose2d distance=Pose2d::relativePose(target,cur);
|
|
|
Pose2d absDiff=Pose2d::abs(distance);
|
|
|
if(nearest_yaw==false) {
|
|
|
@@ -688,7 +717,7 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
|
|
|
if( fabs(target_relative.y()>fabs(target_relative.x()))) //目标轨迹点在当前点Y轴上
|
|
|
directY=true;
|
|
|
}
|
|
|
-
|
|
|
+ printf(" exec along autoDirect:%d\n",autoDirect);
|
|
|
while (cancel_ == false) {
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
|
if (pause_ == true) {
|
|
|
@@ -743,6 +772,11 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
|
|
|
Stop();
|
|
|
return false;
|
|
|
}
|
|
|
+ if(directY==false)
|
|
|
+ SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
|
|
|
+ else
|
|
|
+ SendMoveCmd(move_mode_, Monitor_emqx::eHorizontal, out[0], out[1]);
|
|
|
+ actionType_=eMPC;
|
|
|
/*
|
|
|
* 判断车辆是否在终点附近徘徊，无法到达终点
|
|
|
*/
|
|
|
@@ -755,14 +789,13 @@ bool Navigation::execute_along_action(const Pose2d& begin,const Pose2d& target,c
|
|
|
limitv.max = 0.1;
|
|
|
limitR.min = 2 * M_PI / 180.0;
|
|
|
limitR.max = 10 * M_PI / 180.0;
|
|
|
- if (execute_adjust_action(target, target_diff, limitv, limitv, limitR))
|
|
|
+ Pose2d accuracy(target_diff.x(),target_diff.y(),M_PI*2);
|
|
|
+ if (execute_adjust_action(target, accuracy, limitv, limitv, limitR,false))
|
|
|
return true;
|
|
|
else
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
- SendMoveCmd(move_mode_, Monitor_emqx::eMPC, out[0], out[1]);
|
|
|
- actionType_=eMPC;
|
|
|
//std::this_thread::sleep_for(std::chrono::milliseconds (400));
|
|
|
}
|
|
|
|
|
|
@@ -810,6 +843,52 @@ void Navigation::navigatting()
|
|
|
actionType_=eReady;
|
|
|
printf(" navigation beg...\n");
|
|
|
|
|
|
+ if(global_navCmd_.action()==6)
|
|
|
+ {
|
|
|
+ printf("exec new nav cmd ...\n");
|
|
|
+
|
|
|
+ while(newUnfinished_cations_.empty()==false && cancel_==false)
|
|
|
+ {
|
|
|
+ std::cout<<"unfinished size:"<<newUnfinished_cations_.size()<<std::endl;
|
|
|
+ NavMessage::NewAction act=newUnfinished_cations_.front();
|
|
|
+ if(act.type()==5) {//夹持
|
|
|
+ if(this->clamp_close()==false){
|
|
|
+ printf("夹持failed ...\n");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }else if(act.type()==6){
|
|
|
+ if(this->clamp_open()==false){
|
|
|
+ printf("打开夹持 failed...\n");
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }else if(act.type()==3 || act.type()==4){
|
|
|
+ if(!execute_nodes(act))
|
|
|
+ break;
|
|
|
+ }else{
|
|
|
+ printf(" action type 1/2 not handled !!\n");
|
|
|
+ }
|
|
|
+ newUnfinished_cations_.pop();
|
|
|
+ }
|
|
|
+ actionType_=eReady;
|
|
|
+ Stop();
|
|
|
+ if(cancel_==true) {
|
|
|
+ printf(" navigation canceled\n");
|
|
|
+ while(newUnfinished_cations_.empty()==false)
|
|
|
+ newUnfinished_cations_.pop();
|
|
|
+ }else {
|
|
|
+ if (newUnfinished_cations_.empty())
|
|
|
+ printf("navigation completed!!!\n");
|
|
|
+ else{
|
|
|
+ printf(" navigation Failed\n");
|
|
|
+ while(newUnfinished_cations_.empty()==false)
|
|
|
+ newUnfinished_cations_.pop();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ running_=false;
|
|
|
+ return ;
|
|
|
+ }
|
|
|
+
|
|
|
+ //---------------------
|
|
|
while(unfinished_cations_.empty()==false && cancel_==false)
|
|
|
{
|
|
|
NavMessage::Action act=unfinished_cations_.front();
|
|
|
@@ -827,7 +906,7 @@ void Navigation::navigatting()
|
|
|
limit_angular.max=act.angular_limit().max();
|
|
|
limint_horize.min=act.horize_limit().min();
|
|
|
limint_horize.max=act.horize_limit().max();
|
|
|
- if(execute_adjust_action(target,target_diff,limit_velocity,limint_horize,limit_angular)==false)
|
|
|
+ if(execute_adjust_action(target,target_diff,limit_velocity,limint_horize,limit_angular,false)==false)
|
|
|
{
|
|
|
printf("execute adjust action failed\n");
|
|
|
break;
|
|
|
@@ -842,7 +921,7 @@ void Navigation::navigatting()
|
|
|
printf(" MPC to (%.3f %.3f %.3f) diff:(%.3f,%.3f,%.3f)\n",
|
|
|
act.target().x(),act.target().y(),act.target().theta(),
|
|
|
act.target_diff().x(),act.target_diff().y(),act.target_diff().theta());
|
|
|
- if(execute_along_action(begin,target,target_diff,limit_velocity)==false) {
|
|
|
+ if(execute_along_action(begin,target,target_diff,limit_velocity,false)==false) {
|
|
|
printf("execute along action failed\n");
|
|
|
break;
|
|
|
}
|
