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 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()("help", "Show help message")("host", boost::program_options::value<std::string>(), "Host for the "
00065                                                                                                         "MongoDB.")(
00066       "port", boost::program_options::value<std::size_t>(),
00067       "Port for the MongoDB.")("scene", boost::program_options::value<std::string>(), "Name of scene to publish.")(
00068       "planning_requests", "Also publish the planning requests that correspond to the scene")(
00069       "planning_results", "Also publish the planning results that correspond to the scene")(
00070       "constraint", boost::program_options::value<std::string>(), "Name of constraint to publish.")(
00071       "state", boost::program_options::value<std::string>(),
00072       "Name of the robot state to publish.")("delay", boost::program_options::value<double>()->default_value(delay),
00073                                              "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'",
00125                  pswm->lookupString(moveit_warehouse::PlanningSceneStorage::PLANNING_SCENE_ID_NAME).c_str());
00126         pub_scene.publish(static_cast<const moveit_msgs::PlanningScene&>(*pswm));
00127         ros::spinOnce();
00128 
00129         // publish optional data associated to the scene
00130         if (req || res)
00131         {
00132           std::vector<moveit_warehouse::MotionPlanRequestWithMetadata> planning_queries;
00133           std::vector<std::string> query_names;
00134           pss.getPlanningQueries(planning_queries, query_names, pswm->name);
00135           ROS_INFO("There are %d planning queries associated to the scene", (int)planning_queries.size());
00136           ros::WallDuration(0.5).sleep();
00137           for (std::size_t i = 0; i < planning_queries.size(); ++i)
00138           {
00139             if (req)
00140             {
00141               ROS_INFO("Publishing query '%s'", query_names[i].c_str());
00142               pub_req.publish(static_cast<const moveit_msgs::MotionPlanRequest&>(*planning_queries[i]));
00143               ros::spinOnce();
00144             }
00145             if (res)
00146             {
00147               std::vector<moveit_warehouse::RobotTrajectoryWithMetadata> planning_results;
00148               pss.getPlanningResults(planning_results, query_names[i], pswm->name);
00149               for (std::size_t j = 0; j < planning_results.size(); ++j)
00150               {
00151                 pub_res.publish(static_cast<const moveit_msgs::RobotTrajectory&>(*planning_results[j]));
00152                 ros::spinOnce();
00153               }
00154             }
00155           }
00156         }
00157         wait_time.sleep();
00158       }
00159     }
00160   }
00161 
00162   // publish constraints
00163   if (vm.count("constraint"))
00164   {
00165     moveit_warehouse::ConstraintsStorage cs(vm.count("host") ? vm["host"].as<std::string>() : "",
00166                                             vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00167     pub_constr = nh.advertise<moveit_msgs::Constraints>(CONSTRAINTS_TOPIC, 100);
00168     std::vector<std::string> cnames;
00169     cs.getKnownConstraints(vm["constraint"].as<std::string>(), cnames);
00170 
00171     for (std::size_t i = 0; i < cnames.size(); ++i)
00172     {
00173       moveit_warehouse::ConstraintsWithMetadata cwm;
00174       if (cs.getConstraints(cwm, cnames[i]))
00175       {
00176         ROS_INFO("Publishing constraints '%s'",
00177                  cwm->lookupString(moveit_warehouse::ConstraintsStorage::CONSTRAINTS_ID_NAME).c_str());
00178         pub_constr.publish(static_cast<const moveit_msgs::Constraints&>(*cwm));
00179         ros::spinOnce();
00180         wait_time.sleep();
00181       }
00182     }
00183   }
00184 
00185   // publish constraints
00186   if (vm.count("state"))
00187   {
00188     moveit_warehouse::RobotStateStorage rs(vm.count("host") ? vm["host"].as<std::string>() : "",
00189                                            vm.count("port") ? vm["port"].as<std::size_t>() : 0);
00190     pub_state = nh.advertise<moveit_msgs::RobotState>(STATES_TOPIC, 100);
00191     std::vector<std::string> rnames;
00192     rs.getKnownRobotStates(vm["state"].as<std::string>(), rnames);
00193 
00194     for (std::size_t i = 0; i < rnames.size(); ++i)
00195     {
00196       moveit_warehouse::RobotStateWithMetadata rswm;
00197       if (rs.getRobotState(rswm, rnames[i]))
00198       {
00199         ROS_INFO("Publishing state '%s'", rswm->lookupString(moveit_warehouse::RobotStateStorage::STATE_NAME).c_str());
00200         pub_state.publish(static_cast<const moveit_msgs::RobotState&>(*rswm));
00201         ros::spinOnce();
00202         wait_time.sleep();
00203       }
00204     }
00205   }
00206 
00207   ros::WallDuration(1.0).sleep();
00208   ROS_INFO("Done.");
00209 
00210   return 0;
00211 }


warehouse
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:47