00001 #include <boost/shared_ptr.hpp> 00002 #include <stdexcept> 00003 00004 #include <gazebo_msgs/GetModelState.h> 00005 #include <gazebo_msgs/SetModelState.h> 00006 #include <ros/ros.h> 00007 00008 #include <bwi_msgs/RobotTeleporterInterface.h> 00009 #include <segbot_simulation_apps/common.h> 00010 00011 std::string robot_model_name_; 00012 boost::shared_ptr<ros::ServiceClient> get_gazebo_model_client_; 00013 boost::shared_ptr<ros::ServiceClient> set_gazebo_model_client_; 00014 00015 bool execute(bwi_msgs::RobotTeleporterInterface::Request &req, 00016 bwi_msgs::RobotTeleporterInterface::Response &res) { 00017 00018 res.success = segbot_simulation_apps::teleportEntity(robot_model_name_, 00019 req.pose, 00020 *get_gazebo_model_client_, 00021 *set_gazebo_model_client_); 00022 return true; 00023 } 00024 00025 int main(int argc, char *argv[]) { 00026 00027 ros::init(argc, argv, "gazebo_robot_teleporter"); 00028 ros::NodeHandle nh; 00029 ros::NodeHandle private_nh("~"); 00030 00031 if (!private_nh.getParam("robotid", robot_model_name_)) { 00032 ROS_FATAL_STREAM("GazeboRobotTeleporter: Parameter ~robotid needs to be set to indicate the robot entity name " << 00033 "inside Gazebo."); 00034 return -1; 00035 } 00036 00037 get_gazebo_model_client_.reset(new ros::ServiceClient); 00038 *get_gazebo_model_client_ = nh.serviceClient<gazebo_msgs::GetModelState>( "/gazebo/get_model_state"); 00039 bool gazebo_available = get_gazebo_model_client_->waitForExistence(ros::Duration(30)); 00040 00041 if (!gazebo_available) { 00042 ROS_FATAL_STREAM("GazeboRobotTeleporter: Gazebo is NOT AVAILABLE"); 00043 return -1; 00044 } 00045 00046 set_gazebo_model_client_.reset(new ros::ServiceClient); 00047 *set_gazebo_model_client_ = nh.serviceClient<gazebo_msgs::SetModelState>( "/gazebo/set_model_state"); 00048 set_gazebo_model_client_->waitForExistence(); 00049 00050 ros::ServiceServer service = nh.advertiseService("teleport_robot", execute); 00051 ros::spin(); 00052 00053 return 0; 00054 }