door_handler.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 
00003 #include <bwi_msgs/DoorHandlerInterface.h>
00004 #include <segbot_simulation_apps/door_handler.h>
00005 
00006 boost::shared_ptr<segbot_simulation_apps::DoorHandler> gh_;
00007 
00008 bool execute(bwi_msgs::DoorHandlerInterface::Request  &req,
00009              bwi_msgs::DoorHandlerInterface::Response &res) {
00010 
00011   res.status = "";
00012   if (req.all_doors) {
00013     if (req.open) {
00014       gh_->openAllDoors();
00015     } else {
00016       gh_->closeAllDoors();
00017     }
00018     res.success = true;
00019   } else {
00020     if (req.open) {
00021       res.success = gh_->openDoor(req.door);
00022     } else {
00023       res.success = gh_->closeDoor(req.door);
00024     }
00025     if (!res.success) {
00026       res.status = "Unable to resolved " + req.door + "!";
00027     }
00028   }
00029 
00030   return true;
00031 }
00032 
00033 int main(int argc, char *argv[]) {
00034 
00035   ros::init(argc, argv, "gazebo_door_handler");
00036   ros::NodeHandle nh;
00037 
00038   ros::ServiceServer service = nh.advertiseService("update_doors", execute);
00039   gh_.reset(new segbot_simulation_apps::DoorHandler);
00040   ros::spin();
00041   return 0;
00042 }


segbot_simulation_apps
Author(s): Piyush Khandelwal
autogenerated on Thu Aug 27 2015 15:11:04