broadcast.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/warehouse/planning_scene_storage.h>
00038 #include <moveit/warehouse/constraints_storage.h>
00039 #include <moveit/warehouse/state_storage.h>
00040 
00041 #include <boost/program_options/cmdline.hpp>
00042 #include <boost/program_options/options_description.hpp>
00043 #include <boost/program_options/parsers.hpp>
00044 #include <boost/program_options/variables_map.hpp>
00045 
00046 #include <ros/ros.h>
00047 
00048 static const std::string PLANNING_SCENE_TOPIC = "planning_scene";
00049 static const std::string PLANNING_REQUEST_TOPIC = "motion_plan_request";
00050 static const std::string PLANNING_RESULTS_TOPIC = "motion_plan_results";
00051 
00052 static const std::string CONSTRAINTS_TOPIC = "constraints";
00053 
00054 static const std::string STATES_TOPIC = "robot_states";
00055 
00056 int main(int argc, char **argv)
00057 {
00058   ros::init(argc, argv, "publish_warehouse_data", ros::init_options::AnonymousName);
00059 
00060   // time to wait in between publishing messages
00061   double delay = 0.001;
00062 
00063   boost::program_options::options_description desc;
00064   desc.add_options()
00065     ("help", "Show help message")
00066     ("host", boost::program_options::value<std::string>(), "Host for the MongoDB.")
00067     ("port", boost::program_options::value<std::size_t>(), "Port for the MongoDB.")
00068     ("scene", boost::program_options::value<std::string>(), "Name of scene to publish.")
00069     ("planning_requests", "Also publish the planning requests that correspond to the scene")
00070     ("planning_results", "Also publish the planning results that correspond to the scene")
00071     ("constraint", boost::program_options::value<std::string>(), "Name of constraint to publish.")
00072     ("state", boost::program_options::value<std::string>(), "Name of the robot state to publish.")
00073     ("delay", boost::program_options::value<double>()->default_value(delay), "Time to wait in between publishing messages (s)");
00074 
00075   boost::program_options::variables_map vm;
00076   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), vm);
00077   boost::program_options::notify(vm);
00078 
00079   if (vm.count("help") || (!vm.count("scene") && !vm.count("constraint") && !vm.count("state")))
00080   {
00081     std::cout << desc << std::endl;
00082     return 1;
00083   }
00084   try
00085   {
00086     delay = vm["delay"].as<double>();
00087   }
00088   catch(...)
00089   {
00090     std::cout << desc << std::endl;
00091     return 2;
00092   }
00093 
00094   ros::AsyncSpinner spinner(1);
00095   spinner.start();
00096 
00097   ros::NodeHandle nh;
00098   ros::Publisher pub_scene, pub_req, pub_res, pub_constr, pub_state;
00099   ros::Duration wait_time(delay);
00100 
00101   // publish the scene
00102   if (vm.count("scene"))
00103   {
00104     pub_scene = nh.advertise<moveit_msgs::PlanningScene>(PLANNING_SCENE_TOPIC, 10);
00105     bool req = vm.count("planning_requests");
00106     bool res = vm.count("planning_results");
00107     if (req)
00108       pub_req = nh.advertise<moveit_msgs::MotionPlanRequest>(PLANNING_REQUEST_TOPIC, 100);
00109     if (res)
00110       pub_res = nh.advertise<moveit_msgs::RobotTrajectory>(PLANNING_RESULTS_TOPIC, 100);
00111 
00112     moveit_warehouse::PlanningSceneStorage pss(vm.count("host") ? vm["host"].as<std::string>() : "",
00113                                                vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00114     ros::spinOnce();
00115 
00116     std::vector<std::string> scene_names;
00117     pss.getPlanningSceneNames(vm["scene"].as<std::string>(), scene_names);
00118 
00119     for (std::size_t i = 0 ; i < scene_names.size() ; ++i)
00120     {
00121       moveit_warehouse::PlanningSceneWithMetadata pswm;
00122       if (pss.getPlanningScene(pswm, scene_names[i]))
00123       {
00124         ROS_INFO("Publishing scene '%s'", pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str());
00125         pub_scene.publish(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
00126         ros::spinOnce();
00127 
00128         // publish optional data associated to the scene
00129         if (req || res)
00130         {
00131           std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
00132           std::vector<std::string> query_names;
00133           pss.getPlanningQueries(planning_queries, query_names, pswm->name);
00134           ROS_INFO("There are %d planning queries associated to the scene", (int)planning_queries.size());
00135           ros::WallDuration(0.5).sleep();
00136           for (std::size_t i = 0 ; i < planning_queries.size() ; ++i)
00137           {
00138             if (req)
00139             {
00140               ROS_INFO("Publishing query '%s'", query_names[i].c_str());
00141               pub_req.publish(static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_queries[i]));
00142               ros::spinOnce();
00143             }
00144             if (res)
00145             {
00146               std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
00147               pss.getPlanningResults(planning_results, query_names[i], pswm->name);
00148               for (std::size_t j = 0 ; j < planning_results.size() ; ++j)
00149               {
00150                 pub_res.publish(static_cast<const moveit_msgs::RobotTrajectory&>(*planning_results[j]));
00151                 ros::spinOnce();
00152               }
00153             }
00154           }
00155         }
00156         wait_time.sleep();
00157       }
00158     }
00159   }
00160 
00161   // publish constraints
00162   if (vm.count("constraint"))
00163   {
00164     moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00165                                             vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00166     pub_constr = nh.advertise<moveit_msgs::Constraints>(CONSTRAINTS_TOPIC, 100);
00167     std::vector<std::string> cnames;
00168     cs.getKnownConstraints(vm["constraint"].as<std::string>(), cnames);
00169 
00170     for (std::size_t i = 0 ; i < cnames.size() ; ++i)
00171     {
00172       moveit_warehouse::ConstraintsWithMetadata cwm;
00173       if (cs.getConstraints(cwm, cnames[i]))
00174       {
00175         ROS_INFO("Publishing constraints '%s'", cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str());
00176         pub_constr.publish(static_cast<const moveit_msgs::Constraints&>(*cwm));
00177         ros::spinOnce();
00178         wait_time.sleep();
00179       }
00180     }
00181   }
00182 
00183   // publish constraints
00184   if (vm.count("state"))
00185   {
00186     moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00187                                            vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00188     pub_state = nh.advertise<moveit_msgs::RobotState>(STATES_TOPIC, 100);
00189     std::vector<std::string> rnames;
00190     rs.getKnownRobotStates(vm["state"].as<std::string>(), rnames);
00191 
00192     for (std::size_t i = 0 ; i < rnames.size() ; ++i)
00193     {
00194       moveit_warehouse::RobotStateWithMetadata rswm;
00195       if (rs.getRobotState(rswm, rnames[i]))
00196       {
00197         ROS_INFO("Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
00198         pub_state.publish(static_cast<const moveit_msgs::RobotState&>(*rswm));
00199         ros::spinOnce();
00200         wait_time.sleep();
00201       }
00202     }
00203   }
00204 
00205   ros::WallDuration(1.0).sleep();
00206   ROS_INFO("Done.");
00207 
00208   return 0;
00209 }


warehouse
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 02:32:09