7 #include <boost/asio.hpp> 8 #include <std_srvs/Trigger.h> 9 #include <boost/smart_ptr.hpp> 13 using boost::asio::ip::tcp;
21 int main(
int argc,
char** argv) {
22 ros::init(argc, argv,
"pick_and_place_client");
29 nodeHandlePrivate.
param<std::string>(
"server_ip", ip,
"localhost");
31 boost::asio::io_service io_service;
32 tcp::resolver resolver(io_service);
33 tcp::resolver::query query(tcp::v4(), ip.c_str() ,
"5001");
34 tcp::resolver::iterator iterator = resolver.resolve(query);
35 tcp::socket connection(io_service);
36 connection.connect(*iterator);
39 std::string colorName =
"red";
42 boost::asio::read(connection, boost::asio::buffer(data, 3));
43 if (std::strcmp(data,
"go\n") == 0) {
45 boost::asio::write(connection, boost::asio::buffer(
"go\n", 3));
47 recover(pickAndPlaceClient, connection);
60 std_srvs::Trigger::Request pickAndPlaceReq;
61 std_srvs::Trigger::Response pickAndPlaceRes;
63 if (pickAndPlaceClient.
call(pickAndPlaceReq, pickAndPlaceRes)) {
66 return pickAndPlaceRes;
76 write(client, boost::asio::buffer(
"go\n", 3));
79 }
else if(choice ==
'q') {
87 std::string baseCmd =
"rosrun dynamic_reconfigure dynparam load /find_object_node `rospack find robotican_demos`/config/";
88 if(colorName ==
"red") {
90 FILE *process = popen((baseCmd +
"red_object.yaml").c_str(),
"r");
96 else if(colorName ==
"green") {
97 FILE *process = popen((baseCmd +
"green_object.yaml").c_str(),
"r");
102 }
else if(colorName ==
"blue") {
103 FILE *process = popen((baseCmd +
"blue_object.yaml").c_str(),
"r");
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
ROSCPP_DECL const std::string & getName()
boost::shared_ptr< tcp::socket > socket_ptr
void recover(ros::ServiceClient &pickAndPlaceClient, tcp::socket &client)
std_srvs::Trigger::Response pickAndPlace(ros::ServiceClient &pickAndPlaceClient)
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void changeColor(std::string colorName)
#define ROS_INFO_STREAM(args)
ROSCPP_DECL void shutdown()
int main(int argc, char **argv)