catkin_create_pkg learning_actionlib actionlib message_generation roscpp rospy std_msgs actionlib_msgs
action_client.cpp
/*
下面显示怎样给名字为 "do_dishes"的DoDishes ActionServer 发送goal
*/
#include <learning_actionlib/DoDishesAction.h>
#include <actionlib/client/simple_action_client.h>
typedef actionlib::SimpleActionClient<learning_actionlib::DoDishesAction> Client;
int main(int argc, char** argv)
{
ros::init(argc, argv, "do_dishes_client");
Client client("do_dishes", true); // true -> don't need ros::spin()
client.waitForServer();
learning_actionlib::DoDishesGoal goal;
// Fill in goal here
client.sendGoal(goal);
client.waitForResult(ros::Duration(5.0));
if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
printf("Yay! The dishes are now clean");
printf("Current State: %s\n", client.getState().toString().c_str());
return 0;
}
action_server.cpp
#include <learning_actionlib/DoDishesAction.h>
#include <actionlib/server/simple_action_server.h>
typedef actionlib::SimpleActionServer<learning_actionlib::DoDishesAction> Server;
void execute(const learning_actionlib::DoDishesGoalConstPtr& goal, Server* as)
{
// Do lots of awesome groundbreaking robot stuff here
as->setSucceeded();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "do_dishes_server");
ros::NodeHandle n;
Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
server.start();
ros::spin();
return 0;
}