Go to the documentation of this file.00001 #include <boost/algorithm/string/join.hpp>
00002 #include <boost/foreach.hpp>
00003 #include <boost/regex.hpp>
00004 #include <stdexcept>
00005
00006 #include <bwi_planning_common/utils.h>
00007 #include <bwi_tools/resource_resolver.h>
00008 #include <gazebo_msgs/SpawnModel.h>
00009 #include <gazebo_msgs/GetModelState.h>
00010 #include <gazebo_msgs/SetModelState.h>
00011 #include <tf/transform_datatypes.h>
00012
00013 #include <bwi_tools/point.h>
00014 #include <segbot_simulation_apps/door_handler.h>
00015
00016 namespace segbot_simulation_apps {
00017
00018 DoorHandler::DoorHandler() {
00019
00020 ros::NodeHandle nh, private_nh("~");
00021
00022 std::vector<std::string> unavailable_parameters;
00023 if (!(private_nh.getParam("obstacle_urdf", obstacle_urdf_))) {
00024 unavailable_parameters.push_back("obstacle_urdf");
00025 }
00026 if (!(private_nh.getParam("door_urdf", door_urdf_))) {
00027 unavailable_parameters.push_back("door_urdf");
00028 }
00029
00030 if (unavailable_parameters.size() != 0) {
00031 std::string message = "Following neccessary params not available: " +
00032 boost::algorithm::join(unavailable_parameters, ", ");
00033 ROS_INFO_STREAM(message);
00034 throw std::runtime_error(message);
00035 }
00036
00037 get_gazebo_model_client_ =
00038 nh.serviceClient<gazebo_msgs::GetModelState>(
00039 "/gazebo/get_model_state");
00040 bool gazebo_available =
00041 get_gazebo_model_client_.waitForExistence(ros::Duration(30));
00042
00043 if (!gazebo_available) {
00044 ROS_FATAL_STREAM("ClingoGazeboHandler: Gazebo is NOT AVAILABLE");
00045 throw
00046 std::runtime_error("ClingoGazeboHandler: Gazebo is NOT AVAILABLE");
00047 }
00048
00049 set_gazebo_model_client_ =
00050 nh.serviceClient<gazebo_msgs::SetModelState>(
00051 "/gazebo/set_model_state");
00052 set_gazebo_model_client_.waitForExistence();
00053
00054 spawn_model_client_ =
00055 nh.serviceClient<gazebo_msgs::SpawnModel>(
00056 "/gazebo/spawn_urdf_model");
00057 set_gazebo_model_client_.waitForExistence();
00058
00059 multimap_subscriber_ = nh.subscribe("map_metadata", 1, &DoorHandler::multimapHandler, this);
00060
00061 initialized_ = false;
00062 }
00063
00064 void DoorHandler::initialize() {
00065
00066
00067 door_open_status_.resize(doors_.size());
00068 door_to_true_door_map_.resize(doors_.size());
00069 for (unsigned i = 0; i < doors_.size(); ++i) {
00070
00071 int parent_door = i;
00072 for (unsigned j = 0; j < i; ++j) {
00073 if (door_to_true_door_map_[j] != -1) {
00074
00075 float diffx = doors_[j].door_center.x - doors_[i].door_center.x;
00076 float diffy = doors_[j].door_center.y - doors_[i].door_center.y;
00077 if (sqrtf(diffx*diffx + diffy*diffy) < 0.25f) {
00078 parent_door = j;
00079 break;
00080 }
00081 }
00082 }
00083
00084 door_to_true_door_map_[i] = parent_door;
00085 if (parent_door == i) {
00086
00087 spawnObject(true, i);
00088 door_open_status_[i] = false;
00089 }
00090 }
00091
00092
00093
00094 num_obstacles_ = 0;
00095
00096
00097
00098
00099 initialized_ = true;
00100 }
00101
00102 void DoorHandler::multimapHandler(const multi_level_map_msgs::MultiLevelMapData::ConstPtr& multimap) {
00103
00104 if (!initialized_) {
00105 doors_.clear();
00106
00107 BOOST_FOREACH(const multi_level_map_msgs::LevelMetaData &level, multimap->levels) {
00108 std::string resolved_data_directory = bwi_tools::resolveRosResource(level.data_directory);
00109 std::string door_file = bwi_planning_common::getDoorsFileLocationFromDataDirectory(resolved_data_directory);
00110 std::vector<bwi_planning_common::Door> level_doors;
00111 readDoorFile(door_file, level_doors);
00112 doors_.insert(doors_.end(), level_doors.begin(), level_doors.end());
00113 }
00114
00115 initialize();
00116 }
00117 }
00118
00119 geometry_msgs::Pose DoorHandler::getDefaultLocation(bool is_door, int index) {
00120 geometry_msgs::Pose retval;
00121 retval.position.y = 500.0f + index * 2;
00122 retval.position.z = 0.0f;
00123 retval.orientation.x = 0.0f;
00124 retval.orientation.y = 0.0f;
00125 retval.orientation.z = 0.0f;
00126 retval.orientation.w = 1.0f;
00127 if (is_door) {
00128 retval.position.x = 500.0f;
00129 } else {
00130 retval.position.x = 600.0f;
00131 }
00132 return retval;
00133 }
00134
00135 float DoorHandler::getDoorWidth(int index) {
00136 return 0.75f * doors_[index].width;
00137 }
00138
00139 geometry_msgs::Pose DoorHandler::getDoorLocation(int index) {
00140 geometry_msgs::Pose retval;
00141
00142 retval.position.x = doors_[index].door_center.x;
00143 retval.position.y = doors_[index].door_center.y;
00144 retval.position.z = 0;
00145
00146 bwi::Point2f diff =
00147 (doors_[index].door_corners[0] -
00148 doors_[index].door_corners[1]);
00149 float door_yaw = atan2f(diff.y, diff.x) + M_PI / 2;
00150 retval.orientation = tf::createQuaternionMsgFromYaw(door_yaw);
00151
00152 return retval;
00153 }
00154
00155 bool DoorHandler::openDoor(const std::string& door) {
00156 size_t idx = bwi_planning_common::resolveDoor(door, doors_);
00157 if (idx == bwi_planning_common::NO_DOOR_IDX) {
00158 return false;
00159 }
00160 return openDoor(idx);
00161 }
00162
00163 bool DoorHandler::openDoor(int index) {
00164 if (index >= doors_.size()) {
00165 return false;
00166 }
00167 index = door_to_true_door_map_[index];
00168 if (door_open_status_[index])
00169 return true;
00170 std::string prefix = "auto_door_";
00171 std::string model_name = prefix +
00172 boost::lexical_cast<std::string>(index);
00173 geometry_msgs::Pose pose = getDefaultLocation(true, index);
00174 bool success = teleportEntity(model_name, pose, get_gazebo_model_client_, set_gazebo_model_client_);
00175 door_open_status_[index] = true;
00176 return success;
00177 }
00178
00179 void DoorHandler::openAllDoors() {
00180 for (unsigned i = 0; i < doors_.size(); ++i) {
00181 openDoor(i);
00182 }
00183 }
00184
00185 bool DoorHandler::closeDoor(const std::string& door) {
00186 size_t idx = bwi_planning_common::resolveDoor(door, doors_);
00187 if (idx == bwi_planning_common::NO_DOOR_IDX) {
00188 return false;
00189 }
00190 return closeDoor(idx);
00191 }
00192
00193 bool DoorHandler::closeDoor(int index) {
00194 if (index >= doors_.size()) {
00195 return false;
00196 }
00197 index = door_to_true_door_map_[index];
00198 if (!door_open_status_[index])
00199 return true;
00200 ROS_INFO_STREAM("Closing door " << index);
00201 std::string prefix = "auto_door_";
00202 std::string model_name = prefix +
00203 boost::lexical_cast<std::string>(index);
00204 geometry_msgs::Pose pose = getDoorLocation(index);
00205 bool success = teleportEntity(model_name, pose, get_gazebo_model_client_, set_gazebo_model_client_);
00206 door_open_status_[index] = false;
00207 return success;
00208 }
00209
00210 void DoorHandler::closeAllDoors() {
00211 ROS_INFO_STREAM("Closing all doors");
00212 for (unsigned i = 0; i < doors_.size(); ++i) {
00213 closeDoor(i);
00214 }
00215 }
00216
00217
00218 bool DoorHandler::isDoorOpen(const std::string& door) {
00219 size_t idx = bwi_planning_common::resolveDoor(door, doors_);
00220 if (idx == bwi_planning_common::NO_DOOR_IDX) {
00221 return false;
00222 }
00223 return isDoorOpen(idx);
00224 }
00225
00226 bool DoorHandler::isDoorOpen(int index) {
00227 if (index >= doors_.size()) {
00228 return false;
00229 }
00230 index = door_to_true_door_map_[index];
00231 return door_open_status_[index];
00232 }
00233
00234 void DoorHandler::closeAllDoorsFarAwayFromPoint(
00235 const geometry_msgs::Pose& point, float distance) {
00236 for (unsigned i = 0; i < doors_.size(); ++i) {
00237 if (door_to_true_door_map_[i] != i) {
00238
00239 continue;
00240 }
00241 if (!door_open_status_[i]) {
00242 continue;
00243 }
00244 bool is_door_near = checkClosePoses(point, getDoorLocation(i), distance, false);
00245 if (!is_door_near) {
00246 closeDoor(i);
00247 }
00248 }
00249 }
00250
00251 void DoorHandler::spawnObject(bool is_door, int index) {
00252
00253 gazebo_msgs::SpawnModel spawn;
00254 std::string prefix;
00255 if (is_door) {
00256 prefix = "auto_door_";
00257 spawn.request.model_xml = boost::regex_replace(door_urdf_,
00258 boost::regex("@WIDTH@"),
00259 boost::lexical_cast<std::string>(getDoorWidth(index)));
00260 spawn.request.initial_pose = getDoorLocation(index);
00261 } else {
00262 prefix = "auto_obs_";
00263 index = num_obstacles_;
00264 spawn.request.model_xml = obstacle_urdf_;
00265 spawn.request.initial_pose = getDefaultLocation(false, index);
00266 }
00267
00268 spawn.request.model_name = prefix +
00269 boost::lexical_cast<std::string>(index);
00270
00271 if (spawn_model_client_.call(spawn)) {
00272 if (spawn.response.success) {
00273 ++num_obstacles_;
00274 return;
00275 }
00276 ROS_WARN_STREAM("Received error message while spawning object: " <<
00277 spawn.response.status_message);
00278 }
00279
00280 ROS_ERROR_STREAM("Unable to spawn: " << spawn.request.model_name);
00281 }
00282
00283 }