Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <tf/transform_datatypes.h>
00003 #include <gazebo_msgs/GetModelState.h>
00004 #include <gazebo_msgs/SetModelState.h>
00005
00006 #include <segbot_simulation_apps/common.h>
00007
00008 namespace segbot_simulation_apps {
00009
00010 bool checkClosePoses(const geometry_msgs::Pose& p1,
00011 const geometry_msgs::Pose& p2,
00012 float threshold,
00013 bool check_yaw) {
00014
00015 float dist_diff = sqrtf(pow((p1.position.x - p2.position.x), 2) + pow((p1.position.y - p2.position.y), 2));
00016 if (dist_diff > threshold) {
00017 return false;
00018 }
00019 double yaw1 = tf::getYaw(p1.orientation);
00020 double yaw2 = tf::getYaw(p2.orientation);
00021 if (check_yaw && fabs(yaw1 - yaw2) > 0.1) {
00022 return false;
00023 }
00024
00025 return true;
00026 }
00027
00028 bool teleportEntity(const std::string& entity,
00029 const geometry_msgs::Pose& pose,
00030 ros::ServiceClient& get_gazebo_model_client,
00031 ros::ServiceClient& set_gazebo_model_client) {
00032
00033 int count = 0;
00034 int attempts = 5;
00035 bool location_verified = false;
00036 while (count < attempts and !location_verified) {
00037 gazebo_msgs::GetModelState get_srv;
00038 get_srv.request.model_name = entity;
00039 get_gazebo_model_client.call(get_srv);
00040 location_verified = checkClosePoses(get_srv.response.pose, pose);
00041 if (!location_verified) {
00042 gazebo_msgs::SetModelState set_srv;
00043 set_srv.request.model_state.model_name = entity;
00044 set_srv.request.model_state.pose = pose;
00045 set_gazebo_model_client.call(set_srv);
00046 if (!set_srv.response.success) {
00047 ROS_WARN_STREAM("SetModelState service call failed for " << entity << " to " << pose);
00048 }
00049 }
00050 ++count;
00051 }
00052
00053 if (!location_verified) {
00054 ROS_ERROR_STREAM("Unable to teleport " << entity << " to " << pose
00055 << " despite " << attempts << " attempts.");
00056 return false;
00057 }
00058
00059 return true;
00060 }
00061
00062 }