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