ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Sending multiple Goals to robot by cpp code

asked 2018-03-14 11:44:04 -0500

abdelkrim gravatar image

updated 2018-08-08 17:57:20 -0500

jayess gravatar image

after testing the sending a simpleGoal to my robot i based on http://wiki.ros.org/navigation/Tutori... and it working well i focus now on sending multiple goals when it reached the first one go to the second one ... it's possible if yes help with documentations or example NB: i am programming with cpp :)

Update:

i tried with this what do you think about it

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;


bool one_move(move_base_msgs::MoveBaseGoal goal) {
  goal.target_pose.header.frame_id = "base_link";
  goal.target_pose.header.stamp = ros::Time::now();

      ac.sendGoal(goal);
      ac.waitForResult();
      if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
           return true;
      else
           return false;
        }



int main(int argc, char** argv){
  ros::init(argc, argv, "simple_navigation_goals");

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);

  //wait for the action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal goal[3];


  goal[0].target_pose.pose.position.x = 0.7783;
  goal[0].target_pose.pose.position.y = -0.0390;
  goal[0].target_pose.pose.position.z = 0.0;

  goal[0].target_pose.pose.orientation.x = 0.0;
  goal[0].target_pose.pose.orientation.y = 0.0;
  goal[0].target_pose.pose.orientation.z = 0.0182;
  goal[0].target_pose.pose.orientation.w = 0.99910;

  goal[1].target_pose.pose.position.x = 0.7783;
  goal[1].target_pose.pose.position.y = -0.0390;
  goal[1].target_pose.pose.position.z = 0.0;

  goal[1].target_pose.pose.orientation.x = 0.0;
  goal[1].target_pose.pose.orientation.y = 0.0;
  goal[1].target_pose.pose.orientation.z = 0.0182;
  goal[1].target_pose.pose.orientation.w = 0.99910;

  goal[2].target_pose.pose.position.x = 0.7783;
  goal[2].target_pose.pose.position.y = -0.0390;
  goal[2].target_pose.pose.position.z = 0.0;

  goal[2].target_pose.pose.orientation.x = 0.0;
  goal[2].target_pose.pose.orientation.y = 0.0;
  goal[2].target_pose.pose.orientation.z = 0.0182;
  goal[2].target_pose.pose.orientation.w = 0.99910;

  //we'll send a goal to the robot to move 1 meter forward


 ROS_INFO("Sending goal1");
for(int i=0; i<3; i++)
 {
    if(one_move(goal[i])
     {
      ROS_INFO("Hooray, the %f goal reached",i);
     }
    else 
   {
      ROS_INFO("The the %f goal doesn't reached for some reason",i)
   return 0;
  }     
}
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-03-15 08:04:44 -0500

Jasmin gravatar image

updated 2018-03-15 08:20:57 -0500

Well I think that there are 2 main problems in your code:

  1. There is a redundancy in the instructions, and if you will once need to send more than 2 goals , say 5 or even 10 you'll get a huge code for no reason.

  2. Your code doesn't care if the robot didn't succeed the first goal and passes to the second one anyway.

A better practice would be to:

//create a function taking care about reaching one goal and returning true if it succeed
bool one_move(goal) {
      ...
      ac.sendGoal(goal);
      ac.waitForResult();
      if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
           return true;
      else
           return false;
        }

int main(int argc, char** argv){
       ...
     //define  a list of goals
     //loop over the entered goals and pass to the next goal if OK or break if not.
      ...
     return 0;
        }

Good luck!

edit flag offensive delete link more

Comments

thank you very much :)

abdelkrim gravatar image abdelkrim  ( 2018-03-15 08:34:18 -0500 )edit

You're welcome!

Jasmin gravatar image Jasmin  ( 2018-03-15 16:05:25 -0500 )edit

i modified the code have a look ;)

abdelkrim gravatar image abdelkrim  ( 2018-03-15 16:10:19 -0500 )edit

Theoretically, that's it but, did you try to compile the node and run it. There are some obvious errors like in bool one_move(goal) { , it is needed to specify the input type of the function etc. You need to succeed the compilation and see the output result to know if something is going wrong.

Jasmin gravatar image Jasmin  ( 2018-03-19 02:46:49 -0500 )edit

yeah i see i made the modifications requires now works well thank you very much :)

abdelkrim gravatar image abdelkrim  ( 2018-03-20 12:20:01 -0500 )edit

Happy to help!

Jasmin gravatar image Jasmin  ( 2018-03-20 13:39:39 -0500 )edit

what is the final answer?

aarontan gravatar image aarontan  ( 2018-06-06 18:41:19 -0500 )edit

Hello, is it possible if I could see the revised? I've been trying for days to find a way to make a square or circular path using multiple goals in cpp.

Joseph444 gravatar image Joseph444  ( 2020-06-19 01:26:24 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-03-14 11:44:04 -0500

Seen: 835 times

Last updated: Aug 08 '18