Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <actionlib/client/simple_action_client.h>
00003 #include <actionlib/client/terminal_state.h>
00004 #include <object_msgs/ObjectInfo.h>
00005 #include <object_msgs/ObjectInfoRequest.h>
00006 #include <object_msgs/ObjectInfoResponse.h>
00007 #include <object_msgs/Object.h>
00008 #include <iostream>
00009
00010 #define DEFAULT_REQUEST_OBJECTS_TOPIC "world/request_object"
00011
00012 int main (int argc, char **argv)
00013 {
00014 ros::init(argc, argv, "gazebo_object_info_client");
00015
00016 if (argc != 2)
00017 {
00018 ROS_INFO_STREAM("usage: "<<argv[0]<<" <object-name>");
00019 return 1;
00020 }
00021
00022 ros::NodeHandle n;
00023 std::string REQUEST_OBJECTS_TOPIC;
00024
00025 n.param<std::string>("gazebo_state_plugins/request_object_service", REQUEST_OBJECTS_TOPIC, DEFAULT_REQUEST_OBJECTS_TOPIC);
00026 ROS_INFO("Got objects topic name: <%s>", REQUEST_OBJECTS_TOPIC.c_str());
00027
00028 ros::ServiceClient client = n.serviceClient<object_msgs::ObjectInfo>(REQUEST_OBJECTS_TOPIC);
00029 object_msgs::ObjectInfo srv;
00030 srv.request.name = argv[1];
00031 srv.request.get_geometry=true;
00032
00033 if (client.call(srv) && srv.response.success)
00034 {
00035 ROS_INFO("Result:");
00036 std::cout<<srv.response<<std::endl;
00037 }
00038 else
00039 {
00040 ROS_ERROR("Failed to call service %s, success flag: %i",REQUEST_OBJECTS_TOPIC.c_str(),srv.response.success);
00041 return 1;
00042 }
00043
00044 return 0;
00045 }