intmain(int argc, char** argv) { ros::init(argc, argv, "make_plan_node"); ros::NodeHandle nh; // Init service query for make plan //初始化路径规划服务,服务名称为"move_base_node/make_plan" std::string service_name = "move_base_node/make_plan"; //等待服务空闲,如果已经在运行这个服务,会等到运行结束。 while (!ros::service::waitForService(service_name, ros::Duration(3.0))) { ROS_INFO("Waiting for service move_base/make_plan to become available"); } // 服务的客户端,类型就是 nav_msgs::GetPlan ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true); if (!serviceClient) { ROS_FATAL("Could not initialize get plan service from %s", serviceClient.getService().c_str()); return-1; } nav_msgs::GetPlan srv; //请求服务:规划路线 fillPathRequest(srv.request); if (!serviceClient) { ROS_FATAL("Persistent service connection to %s failed", serviceClient.getService().c_str()); return-1; } ROS_INFO("conntect to %s",serviceClient.getService().c_str()); callPlanningService(serviceClient, srv); }
/* AdvertiseOptions是一个类,在/opt/ros/kinetic/include/ros/advertise_options.h * \brief Constructor * \param _topic Topic to publish on * \param _queue_size Maximum number of outgoing messages to be queued for delivery to subscribers * \param _md5sum The md5sum of the message datatype published on this topic * \param _datatype Datatype of the message published on this topic (eg. "std_msgs/String") * \param _connect_cb Function to call when a subscriber connects to this topic * \param _disconnect_cb Function to call when a subscriber disconnects from this topic */ AdvertiseOptions(const std::string& _topic, uint32_t _queue_size, const std::string& _md5sum, const std::string& _datatype, const std::string& _message_definition, const SubscriberStatusCallback& _connect_cb = SubscriberStatusCallback(), const SubscriberStatusCallback& _disconnect_cb = SubscriberStatusCallback()) : topic(_topic) , queue_size(_queue_size) , md5sum(_md5sum) , datatype(_datatype) , message_definition(_message_definition) , connect_cb(_connect_cb) , disconnect_cb(_disconnect_cb) , callback_queue(0) , latch(false) , has_header(false) {}
if (!impl_) { ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher"); return; }
if (!impl_->isValid()) { ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str()); return; } // 验证消息和发布者的类型是否对应 ROS_ASSERT_MSG(impl_->md5sum_ == "*" || std::string(mt::md5sum<M>(message)) == "*" || impl_->md5sum_ == mt::md5sum<M>(message), "Trying to publish message of type [%s/%s] on a publisher with type [%s/%s]", mt::datatype<M>(message), mt::md5sum<M>(message), impl_->datatype_.c_str(), impl_->md5sum_.c_str());
SerializedMessage m; publish(boost::bind(serializeMessage<M>, boost::ref(message)), m); }
接着再调用
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
voidPublisher::publish(const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m)const { // 验证publisher合法性 if (!impl_) { ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str()); return; } if (!impl_->isValid()) { ROS_ASSERT_MSG(false, "Call to publish() on an invalid Publisher (topic [%s])", impl_->topic_.c_str()); return; } TopicManager::instance()->publish(impl_->topic_, serfunc, m); }
voidTopicManager::publish(const std::string& topic, const boost::function<SerializedMessage(void)>& serfunc, SerializedMessage& m) { boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_); if (isShuttingDown()) { return; }
PublicationPtr p = lookupPublicationWithoutLock(topic); if (p->hasSubscribers() || p->isLatching()) { ROS_DEBUG_NAMED("superdebug", "Publishing message on topic [%s] with sequence number [%d]", p->getName().c_str(), p->getSequence());
// Determine what kinds of subscribers we're publishing to. If they're intraprocess with the same C++ type we can do a no-copy publish. bool nocopy = false; bool serialize = false; // We can only do a no-copy publish if a shared_ptr to the message is provided, and we have type information for it if (m.type_info && m.message) p->getPublishTypes(serialize, nocopy, *m.type_info); else serialize = true; if (!nocopy) { m.message.reset(); m.type_info = 0; } if (serialize || p->isLatching()) { SerializedMessage m2 = serfunc(); m.buf = m2.buf; m.num_bytes = m2.num_bytes; m.message_start = m2.message_start; } p->publish(m);
// If we're not doing a serialized publish we don't need to signal the pollset. The write() // call inside signal() is actually relatively expensive when doing a nocopy publish. if (serialize) { poll_manager_->getPollSet().signal(); } } else { p->incrementSequence(); } }
// One argument is passed, result is "Hello, " + arg. classHelloName :public XmlRpcServerMethod { public: // HelloName是 method 名称 HelloName(XmlRpcServer* s) : XmlRpcServerMethod("HelloName", s) {} voidexecute(XmlRpcValue& params, XmlRpcValue& result) { std::string resultString = "Hello, "; resultString += std::string(params[0]); result = resultString; // 赋值给 result } } helloName(&s); // 类HelloName实例化
// A variable number of arguments are passed, all doubles, result is their sum. classSum :public XmlRpcServerMethod { public: Sum(XmlRpcServer* s) : XmlRpcServerMethod("Sum", s) {} voidexecute(XmlRpcValue& params, XmlRpcValue& result) { int nArgs = params.size(); double sum = 0.0; for (int i=0; i<nArgs; ++i) sum += double(params[i]); result = sum; } } sum(&s);
intmain(int argc, char* argv[]) { if (argc != 2) { std::cerr << "Usage: HelloServer serverPort \n"; return-1; } int port = atoi(argv[1]); // Specify the level of verbosity of informational messages. 0 is no output, 5 is very verbose. XmlRpc::setVerbosity(5);