joint_states_publisher_mock.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <algorithm>
19 #include <mutex>
20 #include <thread>
21 #include <chrono>
22 
23 #include <sensor_msgs/JointState.h>
24 
26 
27 namespace prbt_hardware_support
28 {
29 
30 static const std::string JOINT_STATES_TOPIC_NAME{"/prbt/joint_states"};
31 static constexpr unsigned int JOINT_STATES_TOPIC_QUEUE_SIZE{1};
32 
34 {
36  msg_.name = {"joint1", "joint2"};
37  msg_.position = {0.1, -0.11};
38 }
39 
41 {
42  terminate();
43 }
44 
46 {
47  terminate_ = false;
48  thread_ = std::thread{ [this, move]{ this->start(move); } };
49 }
50 
52 {
53  terminate_ = true;
54  if (thread_.joinable())
55  {
56  thread_.join();
57  }
58 }
59 
60 sensor_msgs::JointStateConstPtr JointStatesPublisherMock::getNextMessage()
61 {
62  std::lock_guard<std::mutex> lock(msg_mutex_);
63  sensor_msgs::JointStateConstPtr msg(new sensor_msgs::JointState(msg_));
64  return msg;
65 }
66 
68 {
69  while (!terminate_)
70  {
71  {
72  std::lock_guard<std::mutex> lock(msg_mutex_);
73  sensor_msgs::JointStateConstPtr msg(new sensor_msgs::JointState(msg_));
75  if (move)
76  {
77  // change positions; reaches limit after > 100 seconds
78  msg_.position.at(0) = std::min(1000.0, msg_.position.at(0)+0.1);
79  }
80  }
81  std::this_thread::sleep_for(std::chrono::milliseconds(10));
82  }
83 }
84 
85 } // namespace prbt_hardware_support
msg
void startAsync(bool move=false)
Start a new thread publishing joint states.
void publish(const boost::shared_ptr< M > &message) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
sensor_msgs::JointStateConstPtr getNextMessage()
Obtain the message that is published next.
static const std::string JOINT_STATES_TOPIC_NAME
static constexpr unsigned int JOINT_STATES_TOPIC_QUEUE_SIZE


prbt_hardware_support
Author(s):
autogenerated on Fri Feb 28 2020 03:16:57