handle_robot_node.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  STDR Simulator - Simple Two DImensional Robot Simulator
3  Copyright (C) 2013 STDR Simulator
4  This program is free software; you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation; either version 3 of the License, or
7  (at your option) any later version.
8  This program is distributed in the hope that it will be useful,
9  but WITHOUT ANY WARRANTY; without even the implied warranty of
10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  GNU General Public License for more details.
12  You should have received a copy of the GNU General Public License
13  along with this program; if not, write to the Free Software Foundation,
14  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
15 
16  Authors :
17  * Manos Tsardoulias, etsardou@gmail.com
18  * Aris Thallas, aris.thallas@gmail.com
19  * Chris Zalidis, zalidis@gmail.com
20 ******************************************************************************/
21 
24 
25 #define USAGE "USAGE: robot_handler add <description.yaml> <x> <y> <theta>\n" \
26 "OR: robot_handler delete <robot_name>\n"\
27 "OR: robot_handler replace <robot_name> <new_x> <new_y> <new_theta>"
28 
35 int main(int argc, char** argv) {
36 
37  ros::init(argc, argv, "robot_spawner", ros::init_options::AnonymousName);
38 
40 
42  if (((argc == 3) || (argc == 6)) && (std::string(argv[1]) == "add")) {
43 
44  stdr_msgs::RobotMsg msg;
45 
46  try {
48  <stdr_msgs::RobotMsg>(std::string(argv[2]));
49  }
51  {
52  ROS_ERROR("[STDR_PARSER] %s", ex.what());
53  return -1;
54  }
55 
56  if (argc == 6) {
57  msg.initialPose.x = atof(argv[3]);
58  msg.initialPose.y = atof(argv[4]);
59  msg.initialPose.theta = atof(argv[5]);
60  }
61 
62  stdr_msgs::RobotIndexedMsg namedRobot;
63 
64  try {
65  namedRobot = handler.spawnNewRobot(msg);
66  return 0;
67  }
69  ROS_ERROR("%s", ex.what());
70  return -1;
71  }
72 
73  }
75  else if ((argc == 3) && (std::string(argv[1]) == "delete")) {
76 
77  std::string name(argv[2]);
78 
79  try {
80  if (handler.deleteRobot(name)) {
81  ROS_INFO("Robot %s deleted successfully", name.c_str());
82  }
83  else {
84  ROS_ERROR("Could not delete robot %s", name.c_str());
85  }
86 
87  return 0;
88  }
90  ROS_ERROR("%s", ex.what());
91  return -1;
92  }
93 
94  }
95 
97  else if ((argc == 6) && (std::string(argv[1]) == "replace")) {
98 
99  std::string name(argv[2]);
100 
101  geometry_msgs::Pose2D newPose;
102  newPose.x = atof(argv[3]);
103  newPose.y = atof(argv[4]);
104  newPose.theta = atof(argv[5]);
105 
106  if (handler.moveRobot(name, newPose)) {
107  ROS_INFO("%s moved to new pose with x: %f, y: %f, theta: %f",
108  name.c_str(), newPose.x, newPose.y, newPose.theta);
109  return 0;
110  }
111 
112  ROS_ERROR("Could not move %s", name.c_str());
113  return -1;
114  }
115  // wrong args
116  else {
117  ROS_ERROR("%s", USAGE);
118  exit(-1);
119  }
120 }
stdr_msgs::RobotIndexedMsg spawnNewRobot(const stdr_msgs::RobotMsg msg)
Spawns a new robot.
static T createMessage(std::string file_name)
Handles the manipulation of robots (creation, deletion, move)
Definition: handle_robot.h:50
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define USAGE
#define ROS_INFO(...)
bool deleteRobot(const std::string &name)
Deletes a robot by frame_id.
Provides a connection exception. Publicly inherits from std::runtime_error. Used in robot handler...
Definition: exceptions.h:33
bool moveRobot(const std::string &name, const geometry_msgs::Pose2D newPose)
Re-places a robot by frame_id.
#define ROS_ERROR(...)
int main(int argc, char **argv)
Main function of the server node.


stdr_robot
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Mon Jun 10 2019 15:15:10