control_base.hpp

Go to the documentation of this file.

// Copyright (c) 2019 Intel Corporation. All Rights Reserved
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <mutex>
#include <thread>
#include <Eigen/Geometry>
#include <rclcpp/rclcpp.hpp>
#include <tf2_eigen/tf2_eigen.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_ros/static_transform_broadcaster.h>

struct TcpPose
{
  double x; 
  double y; 
  double z; 
  double alpha; 
  double beta;  
  double gamma; 
};

class ArmControlBase: public rclcpp::Node
{
public:

  ArmControlBase(const std::string node_name, const rclcpp::NodeOptions & options)
  : Node(node_name, options), broadcaster_(this)
  {
    joint_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 1);
    time_out_ = 15.0;

    tf_msg_.header.frame_id = "base"; // Used to void TF_NO_FRAME_ID error, updated by user later
    tf_msg_.child_frame_id = "pose_goal";
    // Initialize rotation to avoid TF_DENORMALIZED_QUATERNION error
    tf_msg_.transform.rotation.x = 0.0;
    tf_msg_.transform.rotation.y = 0.0;
    tf_msg_.transform.rotation.z = 0.0;
    tf_msg_.transform.rotation.w = 1.0;
    tf_thread_ = std::thread(&ArmControlBase::publishTFGoal, this);
  }

  virtual ~ArmControlBase()
  {
    rclcpp::shutdown();
    tf_thread_.join();
  }

  virtual bool moveToTcpPose(double x, double y, double z, 
                             double alpha, double beta, double gamma, 
                             double vel, double acc) = 0;

  virtual bool moveToTcpPose(const Eigen::Isometry3d& pose, double vel, double acc);

  virtual bool moveToTcpPose(const geometry_msgs::msg::PoseStamped& pose_stamped, double vel, double acc);

  virtual bool moveToJointValues(const std::vector<double>& joint_values, double vel, double acc) = 0;

  virtual bool open(const double distance = 0) = 0;

  virtual bool close(const double distance = 0) = 0;

  virtual bool pick(double x, double y, double z, 
                    double alpha, double beta, double gamma, 
                    double vel, double acc, double vel_scale, double approach);
  
  virtual bool pick(const geometry_msgs::msg::PoseStamped& pose_stamped, 
                    double vel, double acc, double vel_scale, double approach);

  virtual bool place(double x, double y, double z, 
                     double alpha, double beta, double gamma,
                     double vel, double acc, double vel_scale, double retract);

  virtual bool place(const geometry_msgs::msg::PoseStamped& pose_stamped,
                     double vel, double acc, double vel_scale, double retract);

  void toTcpPose(const geometry_msgs::msg::PoseStamped& pose_stamped, TcpPose& tcp_pose);

  void toTcpPose(const Eigen::Isometry3d& pose, TcpPose& tcp_pose);

  virtual bool checkTcpGoalArrived(Eigen::Isometry3d& tcp_goal);

  virtual bool checkJointValueGoalArrived(const std::vector<double>& joint_goal);

  virtual void parseArgs() = 0;

  virtual bool startLoop() = 0;

  virtual void publishTFGoal();

  void updateTFGoal(const geometry_msgs::msg::PoseStamped& pose_stamped);

  Eigen::Vector3d getUnitApproachVector(const double& alpha, const double& beta, const double& gamma);

protected:
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
  std::vector<std::string> joint_names_;
  TcpPose tcp_pose_;
  std::vector<double> joint_values_;
  std::mutex m_;
  double time_out_;
  std::thread tf_thread_;
  geometry_msgs::msg::TransformStamped tf_msg_;
  tf2_ros::StaticTransformBroadcaster broadcaster_;
};