handle_robot_node.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002    STDR Simulator - Simple Two DImensional Robot Simulator
00003    Copyright (C) 2013 STDR Simulator
00004    This program is free software; you can redistribute it and/or modify
00005    it under the terms of the GNU General Public License as published by
00006    the Free Software Foundation; either version 3 of the License, or
00007    (at your option) any later version.
00008    This program is distributed in the hope that it will be useful,
00009    but WITHOUT ANY WARRANTY; without even the implied warranty of
00010    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00011    GNU General Public License for more details.
00012    You should have received a copy of the GNU General Public License
00013    along with this program; if not, write to the Free Software Foundation,
00014    Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
00015    
00016    Authors : 
00017    * Manos Tsardoulias, etsardou@gmail.com
00018    * Aris Thallas, aris.thallas@gmail.com
00019    * Chris Zalidis, zalidis@gmail.com 
00020 ******************************************************************************/
00021 
00022 #include <stdr_robot/handle_robot.h>
00023 #include <stdr_parser/stdr_parser.h>
00024 
00025 #define USAGE "USAGE: robot_handler add <description.yaml> <x> <y> <theta>\n" \
00026 "OR: robot_handler delete <robot_name>\n"\
00027 "OR: robot_handler replace <robot_name> <new_x> <new_y> <new_theta>"
00028 
00035 int main(int argc, char** argv) {
00036   
00037   ros::init(argc, argv, "robot_spawner", ros::init_options::AnonymousName);
00038   
00039   stdr_robot::HandleRobot handler;
00040   
00042   if (((argc == 3) || (argc == 6)) && (std::string(argv[1]) == "add")) {
00043     
00044     stdr_msgs::RobotMsg msg;
00045     
00046     try {
00047       msg = stdr_parser::Parser::createMessage
00048         <stdr_msgs::RobotMsg>(std::string(argv[2]));
00049     }
00050     catch(stdr_parser::ParserException& ex)
00051     {
00052       ROS_ERROR("[STDR_PARSER] %s", ex.what());
00053       return -1;
00054     }
00055     
00056     if (argc == 6) {
00057       msg.initialPose.x = atof(argv[3]);
00058       msg.initialPose.y = atof(argv[4]);
00059       msg.initialPose.theta = atof(argv[5]);
00060     }
00061     
00062     stdr_msgs::RobotIndexedMsg namedRobot;
00063     
00064     try {
00065       namedRobot = handler.spawnNewRobot(msg);
00066       return 0;
00067     }
00068     catch (stdr_robot::ConnectionException& ex) {
00069       ROS_ERROR("%s", ex.what());
00070       return -1;
00071     }
00072     
00073   }
00075   else if ((argc == 3) && (std::string(argv[1]) == "delete")) {
00076     
00077     std::string name(argv[2]);
00078     
00079     try {
00080       if (handler.deleteRobot(name)) {
00081         ROS_INFO("Robot %s deleted successfully", name.c_str());
00082       }
00083       else {
00084         ROS_ERROR("Could not delete robot %s", name.c_str());
00085       }
00086       
00087       return 0;
00088     }
00089     catch (stdr_robot::ConnectionException& ex) {
00090       ROS_ERROR("%s", ex.what());
00091       return -1;
00092     }
00093     
00094   }
00095   
00097   else if ((argc == 6) && (std::string(argv[1]) == "replace")) {
00098     
00099     std::string name(argv[2]);
00100     
00101     geometry_msgs::Pose2D newPose;
00102     newPose.x = atof(argv[3]);
00103     newPose.y = atof(argv[4]);
00104     newPose.theta = atof(argv[5]);
00105     
00106     if (handler.moveRobot(name, newPose)) {
00107       ROS_INFO("%s moved to new pose with x: %f, y: %f, theta: %f", 
00108         name.c_str(), newPose.x, newPose.y, newPose.theta);
00109       return 0;
00110     }
00111     
00112     ROS_ERROR("Could not move %s", name.c_str());
00113     return -1;
00114   }
00115   // wrong args
00116   else {
00117     ROS_ERROR("%s", USAGE);
00118     exit(-1);
00119   }
00120 }


stdr_robot
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Wed Sep 2 2015 03:36:25