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>" 35 int main(
int argc,
char** argv) {
42 if (((argc == 3) || (argc == 6)) && (std::string(argv[1]) ==
"add")) {
44 stdr_msgs::RobotMsg msg;
48 <stdr_msgs::RobotMsg>(std::string(argv[2]));
57 msg.initialPose.x = atof(argv[3]);
58 msg.initialPose.y = atof(argv[4]);
59 msg.initialPose.theta = atof(argv[5]);
62 stdr_msgs::RobotIndexedMsg namedRobot;
75 else if ((argc == 3) && (std::string(argv[1]) ==
"delete")) {
77 std::string name(argv[2]);
81 ROS_INFO(
"Robot %s deleted successfully", name.c_str());
84 ROS_ERROR(
"Could not delete robot %s", name.c_str());
97 else if ((argc == 6) && (std::string(argv[1]) ==
"replace")) {
99 std::string name(argv[2]);
101 geometry_msgs::Pose2D newPose;
102 newPose.x = atof(argv[3]);
103 newPose.y = atof(argv[4]);
104 newPose.theta = atof(argv[5]);
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);
112 ROS_ERROR(
"Could not move %s", name.c_str());
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)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
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...
bool moveRobot(const std::string &name, const geometry_msgs::Pose2D newPose)
Re-places a robot by frame_id.
int main(int argc, char **argv)
Main function of the server node.