moveit2
The MoveIt Motion Planning Framework for ROS 2.
synchronized_string_parameter.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021, PickNik Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of PickNik Robotics nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: David V. Lu!! */
36 
38 
39 namespace rdf_loader
40 {
41 std::string SynchronizedStringParameter::loadInitialValue(const std::shared_ptr<rclcpp::Node>& node,
42  const std::string& name,
43  const StringCallback& parent_callback,
44  bool default_continuous_value, double default_timeout)
45 {
46  node_ = node;
47  name_ = name;
48  parent_callback_ = parent_callback;
49 
50  if (getMainParameter())
51  {
52  if (shouldPublish())
53  {
54  // Transient local is similar to latching in ROS 1.
55  string_publisher_ = node_->create_publisher<std_msgs::msg::String>(name_, rclcpp::QoS(1).transient_local());
56 
57  std_msgs::msg::String msg;
58  msg.data = content_;
59  string_publisher_->publish(msg);
60  }
61  return content_;
62  }
63 
64  // Load topic parameters
65  std::string keep_open_param = name_ + "_continuous";
66  if (!node_->has_parameter(keep_open_param))
67  {
68  node_->declare_parameter(keep_open_param, rclcpp::ParameterType::PARAMETER_BOOL);
69  }
70  bool keep_open;
71  node_->get_parameter_or(keep_open_param, keep_open, default_continuous_value);
72 
73  std::string timeout_param = name_ + "_timeout";
74  if (!node_->has_parameter(timeout_param))
75  {
76  node_->declare_parameter(timeout_param, rclcpp::ParameterType::PARAMETER_DOUBLE);
77  }
78  double d_timeout;
79  node_->get_parameter_or(timeout_param, d_timeout, default_timeout); // ten second default
80  rclcpp::Duration timeout = rclcpp::Duration::from_seconds(d_timeout);
81 
82  if (!waitForMessage(timeout))
83  {
84  RCLCPP_ERROR_ONCE(node_->get_logger(),
85  "Could not find parameter %s and did not receive %s via std_msgs::msg::String subscription "
86  "within %f seconds.",
87  name_.c_str(), name_.c_str(), d_timeout);
88  }
89  if (!keep_open)
90  {
91  string_subscriber_.reset();
92  }
93  return content_;
94 }
95 
97 {
98  // Check if the parameter is declared, declare it if it's not declared yet
99  if (!node_->has_parameter(name_))
100  {
101  node_->declare_parameter(name_, rclcpp::ParameterType::PARAMETER_STRING);
102  }
103 
104  node_->get_parameter_or(name_, content_, std::string());
105 
106  return !content_.empty();
107 }
108 
110 {
111  std::string publish_param = "publish_" + name_;
112  bool publish_string;
113  if (!node_->has_parameter(publish_param))
114  {
115  node_->declare_parameter(publish_param, rclcpp::ParameterType::PARAMETER_BOOL);
116  }
117  node_->get_parameter_or(publish_param, publish_string, false);
118  return publish_string;
119 }
120 
121 bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration& timeout)
122 {
123  auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_);
124  auto const temp_node = std::make_shared<rclcpp::Node>(nd_name);
125  string_subscriber_ = temp_node->create_subscription<std_msgs::msg::String>(
126  name_,
127  rclcpp::QoS(1).transient_local().reliable(), // "transient_local()" is required for supporting late subscriptions
128  [this](const std_msgs::msg::String::ConstSharedPtr& msg) { return stringCallback(msg); });
129 
130  rclcpp::WaitSet wait_set;
131  wait_set.add_subscription(string_subscriber_);
132 
133  auto ret = wait_set.wait(timeout.to_chrono<std::chrono::duration<double>>());
134  if (ret.kind() == rclcpp::WaitResultKind::Ready)
135  {
136  std_msgs::msg::String msg;
137  rclcpp::MessageInfo info;
138  if (string_subscriber_->take(msg, info))
139  {
140  content_ = msg.data;
141  return true;
142  }
143  }
144  return false;
145 }
146 
147 void SynchronizedStringParameter::stringCallback(const std_msgs::msg::String::ConstSharedPtr& msg)
148 {
149  if (msg->data == content_)
150  {
151  return;
152  }
153  if (parent_callback_)
154  {
155  parent_callback_(msg->data);
156  }
157  content_ = msg->data;
158 }
159 } // namespace rdf_loader
rclcpp::Subscription< std_msgs::msg::String >::SharedPtr string_subscriber_
std::string loadInitialValue(const std::shared_ptr< rclcpp::Node > &node, const std::string &name, const StringCallback &parent_callback={}, bool default_continuous_value=false, double default_timeout=10.0)
bool waitForMessage(const rclcpp::Duration &timeout)
rclcpp::Publisher< std_msgs::msg::String >::SharedPtr string_publisher_
void stringCallback(const std_msgs::msg::String::ConstSharedPtr &msg)
std::function< void(const std::string &)> StringCallback
name
Definition: setup.py:7