joint_state_publisher_mock.h

Go to the documentation of this file.

/*
 * Copyright (c) 2020 Pilz GmbH & Co. KG
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef PILZ_TESTUTILS_JOINT_STATE_PUBLISHER_MOCK_H
#define PILZ_TESTUTILS_JOINT_STATE_PUBLISHER_MOCK_H
 
#include <atomic>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include <ros/ros.h>
 
#include <sensor_msgs/JointState.h>
 
namespace pilz_testutils
{
class JointStatePublisherMock
{
  typedef sensor_msgs::JointState JointState;
  typedef sensor_msgs::JointStateConstPtr JointStateConstPtr;
 
public:
  JointStatePublisherMock();
 
  ~JointStatePublisherMock();
 
  JointStatePublisherMock(const JointStatePublisherMock& other) = delete;
  JointStatePublisherMock& operator=(const JointStatePublisherMock& other) = delete;
 
  void startPublishingAsync(const double& joint1_start_position = 0.0);
 
  void setJoint1Velocity(const double& vel);
 
  void goHome();
 
  void stopPublishing();
 
  JointStateConstPtr getNextMessage();
 
private:
  void run();
  void createNextMessage();
  void publish();
  void updateNextMessage();
  void updateJoint1Position();
 
private:
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  std::vector<std::string> joint_names_;
  std::atomic_bool stop_flag_;
  std::atomic_bool go_home_flag_;
  std::thread publisher_thread_;
  double joint1_position_{ 0.0 };
  double joint1_velocity_{ 0.0 };
  ros::Time next_time_stamp_;
  JointState next_msg_;
  std::mutex next_msg_mutex_;
};
 
}  // namespace pilz_testutils
 
#endif