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

Revision history [back]

click to hide/show revision 1
initial version

I know this is an old question, but I thought I would post my answer since I just had to figure this out for myself.

In the above code the node for the action client, and the goal type are incorrectly defined. The node should be dock_drive_action and the goal type should be kobuki_msgs::AutoDockingGoal. Below is a working version of some code to call the docking action:

  // Create the client
  actionlib::SimpleActionClient<kobuki_msgs::AutoDockingAction> docking_ac("dock_drive_action", true);
  docking_ac.waitForServer();

  // Create goal object
  kobuki_msgs::AutoDockingGoal goal;

  // Assign initial state
  actionlib::SimpleClientGoalState dock_state = actionlib::SimpleClientGoalState::LOST;

  // Send the goal
  docking_ac.sendGoal(goal);

  ros::Time time = ros::Time::now();

  // Monitor progress
  while (!docking_ac.waitForResult(ros::Duration(3))) {

    dock_state = docking_ac.getState();
    ROS_INFO("Docking status: %s",dock_state.toString().c_str());

    if (ros::Time::now() > (time+ros::Duration(10))) {
      ROS_INFO("Docking took more than 10 seconds, canceling.");
      docking_ac.cancelGoal();
      break;
    }// end if
  }// end while

Hopefully someone can find this useful.