qb_device_control.h

Go to the documentation of this file.

/***
 *  Software License Agreement: BSD 3-Clause License
 *
 *  Copyright (c) 2016-2018, qbrobotics®
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *    following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
 *    following disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote
 *    products derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 *  WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef QB_DEVICE_CONTROL_H
#define QB_DEVICE_CONTROL_H

// Standard libraries
#include <regex>
#include <mutex>

// ROS libraries
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <controller_manager/controller_manager.h>
#include <combined_robot_hw/combined_robot_hw.h>
#include <ros/callback_queue.h>
#include <std_msgs/Int32.h>

// internal libraries
#include <qb_device_hardware_interface/qb_device_hardware_interface.h>

namespace qb_device_control {
class qbDeviceControl {
 public:
  qbDeviceControl();

  virtual ~qbDeviceControl();

  void move(const trajectory_msgs::JointTrajectory &joint_trajectory, const std::string &controller);

  std::vector<trajectory_msgs::JointTrajectoryPoint> getSinusoidalPoints(const double &amplitude, const double &period, const int &samples_per_period, const int &periods);

  std::vector<trajectory_msgs::JointTrajectoryPoint> getTrapezoidalPoints(const double &amplitude, const double &period, const double &ramp_duration, const int &periods);

  trajectory_msgs::JointTrajectory getCustomTrajectory(const std::vector<std::vector<trajectory_msgs::JointTrajectoryPoint>> &joint_points, const std::string &controller);

  trajectory_msgs::JointTrajectory getWaypointTrajectory(ros::NodeHandle &node_handle, const std::string &controller);

  bool waitForResult(const ros::Duration &timeout, const std::string &controller);

 protected:
  ros::CallbackQueuePtr callback_queue_;
  ros::AsyncSpinner spinner_;
  ros::NodeHandle node_handle_;
  ros::NodeHandle node_handle_control_;
  ros::Publisher frequency_publisher_;
  ros::ServiceClient get_measurements_client_;
  ros::ServiceClient set_commands_client_;
  ros::ServiceClient set_pid_client_;
  ros::ServiceServer get_async_measurements_server_;
  ros::ServiceServer set_async_commands_server_;
  ros::ServiceServer set_async_pid_server_;
  ros::WallTimer control_setup_timer_;
  ros::WallTimer control_timer_;
  ros::WallTimer frequency_timer_;
  ros::WallDuration control_duration_;

  std::mutex sync_protector_;
  std::vector<std::string> device_names_;
  std::vector<std::string> controllers_;
  std::map<std::string, std::string> controller_device_name_;
  std::map<std::string, std::vector<std::string>> controller_joints_;
  std::map<std::string, std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction>>> action_clients_;
  std::map<std::string, trajectory_msgs::JointTrajectory> joint_trajectories_;

  // do not change the following variables order
  combined_robot_hw::CombinedRobotHW devices_;
  bool init_success_;
  controller_manager::ControllerManager controller_manager_;

  void move();

 private:
  int counter_;  // control loop counter (just to check the frequency)

  void actionActiveCallback(const std::string &controller);

  void actionDoneCallback(const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result, const std::string &controller);

  void actionFeedbackCallback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller);

  void controlCallback(const ros::WallTimerEvent &timer_event);

  void controlSetupCallback(const ros::WallTimerEvent &timer_event);

  std::string extractDeviceName(const std::string &controller);

  void frequencyCallback(const ros::WallTimerEvent &timer_event);

  bool getAsyncMeasurementsCallback(qb_device_srvs::GetMeasurementsRequest &request, qb_device_srvs::GetMeasurementsResponse &response);

  void initActionClients();

  bool parseVector(const XmlRpc::XmlRpcValue &xml_value, const std::string &controller, std::vector<double> &vector);

  void parseWaypoints();

  bool setAsyncCommandsCallback(qb_device_srvs::SetCommandsRequest &request, qb_device_srvs::SetCommandsResponse &response);

  bool setAsyncPIDCallback(qb_device_srvs::SetPIDRequest &request, qb_device_srvs::SetPIDResponse &response);

  void update(const ros::WallTime &time, const ros::WallDuration &period);

  template<class T> T xmlCast(XmlRpc::XmlRpcValue xml_value);
};
}  // namespace qb_device_control

#endif // QB_DEVICE_CONTROL_H