00001 // 00002 // Created by tom on 20/09/16. 00003 // 00004 00005 00006 #include <ros/ros.h> 00007 #include <boost/asio.hpp> 00008 #include <std_srvs/Trigger.h> 00009 #include <boost/smart_ptr.hpp> 00010 00011 00012 #define MAX_LEN 128 00013 using boost::asio::ip::tcp; 00014 00015 typedef boost::shared_ptr<tcp::socket> socket_ptr; 00016 00017 std_srvs::Trigger::Response pickAndPlace(ros::ServiceClient &pickAndPlaceClient); 00018 void recover(ros::ServiceClient &pickAndPlaceClient, tcp::socket &client); 00019 void changeColor(std::string colorName); 00020 00021 int main(int argc, char** argv) { 00022 ros::init(argc, argv, "pick_and_place_client"); 00023 ros::NodeHandle nodeHandle; 00024 ros::NodeHandle nodeHandlePrivate("~"); 00025 ros::ServiceClient pickAndPlaceClient = nodeHandle.serviceClient<std_srvs::Trigger>("pick_go"); 00026 pickAndPlaceClient.waitForExistence(); 00027 00028 std::string ip; 00029 nodeHandlePrivate.param<std::string>("server_ip", ip, "localhost"); 00030 00031 boost::asio::io_service io_service; 00032 tcp::resolver resolver(io_service); 00033 tcp::resolver::query query(tcp::v4(), ip.c_str() , "5001"); 00034 tcp::resolver::iterator iterator = resolver.resolve(query); 00035 tcp::socket connection(io_service); 00036 connection.connect(*iterator); 00037 00038 ROS_INFO("[%s]: Connected to server", ros::this_node::getName().c_str()); 00039 std::string colorName = "red"; 00040 while(ros::ok()) { 00041 char data[MAX_LEN] = {'\0'}; 00042 boost::asio::read(connection, boost::asio::buffer(data, 3)); 00043 if (std::strcmp(data, "go\n") == 0) { 00044 if (pickAndPlace(pickAndPlaceClient).success) { 00045 boost::asio::write(connection, boost::asio::buffer("go\n", 3)); 00046 } else { 00047 recover(pickAndPlaceClient, connection); 00048 } 00049 } else { 00050 ROS_WARN("[%s]: Unknown syntax: %s", ros::this_node::getName().c_str(), data); 00051 } 00052 } 00053 00054 00055 00056 return 0; 00057 } 00058 00059 std_srvs::Trigger::Response pickAndPlace(ros::ServiceClient &pickAndPlaceClient) { 00060 std_srvs::Trigger::Request pickAndPlaceReq; 00061 std_srvs::Trigger::Response pickAndPlaceRes; 00062 00063 if (pickAndPlaceClient.call(pickAndPlaceReq, pickAndPlaceRes)) { 00064 ROS_INFO_STREAM(pickAndPlaceRes); 00065 } 00066 return pickAndPlaceRes; 00067 } 00068 00069 void recover(ros::ServiceClient &pickAndPlaceClient, tcp::socket &client) { 00070 ROS_WARN("[%s]: plan failed type 'r' to re-plan or 'q' to quit", ros::this_node::getName().c_str()); 00071 char choice; 00072 do { 00073 std::cin >> choice; 00074 if(choice == 'r') { 00075 if (pickAndPlace(pickAndPlaceClient).success) { 00076 write(client, boost::asio::buffer("go\n", 3)); 00077 break; 00078 } 00079 } else if(choice == 'q') { 00080 ros::shutdown(); 00081 break; 00082 } 00083 } while(ros::ok()); 00084 } 00085 00086 void changeColor(std::string colorName) { 00087 std::string baseCmd = "rosrun dynamic_reconfigure dynparam load /find_object_node `rospack find robotican_demos`/config/"; 00088 if(colorName == "red") { 00089 00090 FILE *process = popen((baseCmd + "red_object.yaml").c_str(), "r"); 00091 ros::Duration(1.5).sleep(); 00092 if(process != 0) { 00093 ROS_INFO("[%s]: Change to red", ros::this_node::getName().c_str()); 00094 } 00095 } 00096 else if(colorName == "green") { 00097 FILE *process = popen((baseCmd + "green_object.yaml").c_str(), "r"); 00098 ros::Duration(1.5).sleep(); 00099 if(process != 0) { 00100 ROS_INFO("[%s]: Change to green", ros::this_node::getName().c_str()); 00101 } 00102 } else if(colorName == "blue") { 00103 FILE *process = popen((baseCmd + "blue_object.yaml").c_str(), "r"); 00104 ros::Duration(1.5).sleep(); 00105 if(process != 0) { 00106 ROS_INFO("[%s]: Change to blue", ros::this_node::getName().c_str()); 00107 } 00108 } 00109 }