How to cancel all goals in my own action server rightly?
Hi,I'm whriting an action to realize multi target navigation and I want to cancel all goals after sending instruction.However,whether I send actiontopic/cancel an empty message or use client.cancelAllGoals() ,it's all useless.How to do it rightly? This is my partial code,is it because I called move_base_client?
void TranslateJson(const string strData,Server *res,tasks_resolution::taskactionFeedback &feedback,tasks_resolution::taskactionResult &result){
MoveBaseClient ac("move_base",true);
for(int k = 0 ;k<nPatrolAction;k++){
if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==1)
{
actiontypeid = START;
}
if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==2)
{
actiontypeid = MOVING;
}
if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==3)
{
actiontypeid = WAITING;
}
if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==4)
{
actiontypeid = RFID_OPEN;
}
if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==5)
{
actiontypeid = RFID_CLOSE;
}
switch (actiontypeid)
{
case START:
break;
case MOVING:
{for(int m = 0;m<narraypoint;m++){
if(this_floor_navigation_goals.pointinfoArray[m].NavigationPointId == taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].NavigationPointId){
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = this_floor_navigation_goals.pointinfoArray[m].Pose.position.x;
goal.target_pose.pose.position.y = this_floor_navigation_goals.pointinfoArray[m].Pose.position.y;
goal.target_pose.pose.position.z = this_floor_navigation_goals.pointinfoArray[m].Pose.position.x;
goal.target_pose.pose.orientation.x = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.x;
goal.target_pose.pose.orientation.y = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.y;
goal.target_pose.pose.orientation.z = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.z;
goal.target_pose.pose.orientation.w = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.w;
ac.sendGoal(goal);
ROS_INFO("the car is moving");
feedback.feedback = ("Now is going to %s",this_floor_navigation_goals.pointinfoArray[m].NavigatePointName);
res->publishFeedback(feedback);
ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
feedback.feedback = "the car has arrived rule point";
res->publishFeedback(feedback);
}else{
feedback.feedback = "the car can not reach the goal";
res->publishFeedback(feedback);
}
}
}
tasks_resolution::othermsgs othermsgs;
othermsgs.ActionTypeId = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId;
othermsgs.WaitingMilliseconds = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].WaitingMilliseconds;
pub_other_msgs.publish(othermsgs);}
break;
case WAITING:{
int sleep_time = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].WaitingMilliseconds/1000;
cout<<"Sleep TIME:"<<sleep_time<<endl;
feedback.feedback = ("Waiting Time:%d s",sleep_time);
res->publishFeedback(feedback);
ros::Duration(sleep_time).sleep();
result.result = "Finish Sleeped.";
res->setSucceeded(result);
}
case RFID_OPEN:
break;
case RFID_CLOSE:
break;
default:
ROS_INFO("Default!");
break;
}
}
}
void execute(const tasks_resolution::taskactionGoalConstPtr& demo_goal,Server* as){
ros::Rate r(2);
string goal = demo_goal->goal;
tasks_resolution::taskactionFeedback feedback;
tasks_resolution::taskactionResult result;
TranslateJson(goal,as,feedback,result);
}
int main(int argc,char **argv){
ros::init(argc,argv,"dodishes");
ros::NodeHandle nh;
Server server(nh,"do_dishes",boost::bind(&execute,_1,&server),false);
server.start();
ros::spin();
return 0 ;
}