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