Program Listing for File datatypes.hpp

↰ Return to documentation for file (include/moveit_servo/utils/datatypes.hpp)

/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2019, Los Alamos National Security, LLC
 * 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.
 *******************************************************************************/

/*      Title       : datatypes.hpp
 *      Project     : moveit_servo
 *      Created     : 06/05/2023
 *      Author      : Andy Zelenak, V Mohammed Ibrahim
 *
 *      Description : The custom datatypes used by Moveit Servo.
 */

#pragma once

#include <string>
#include <tf2_eigen/tf2_eigen.hpp>
#include <unordered_map>

namespace moveit_servo
{

enum class StatusCode : int8_t
{
  INVALID = -1,
  NO_WARNING = 0,
  DECELERATE_FOR_APPROACHING_SINGULARITY = 1,
  HALT_FOR_SINGULARITY = 2,
  DECELERATE_FOR_LEAVING_SINGULARITY = 3,
  DECELERATE_FOR_COLLISION = 4,
  HALT_FOR_COLLISION = 5,
  JOINT_BOUND = 6
};

const std::unordered_map<StatusCode, std::string> SERVO_STATUS_CODE_MAP(
    { { StatusCode::INVALID, "Invalid" },
      { StatusCode::NO_WARNING, "No warnings" },
      { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" },
      { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" },
      { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" },
      { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" },
      { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" },
      { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } });

// The datatype that specifies the type of command that servo should expect.
enum class CommandType : int8_t
{
  JOINT_JOG = 0,
  TWIST = 1,
  POSE = 2,

  // Range of allowed values used for validation.
  MIN = JOINT_JOG,
  MAX = POSE
};

typedef std::pair<StatusCode, Eigen::VectorXd> JointDeltaResult;

// The joint jog command, this will be vector of length equal to the number of joints of the robot.
struct JointJogCommand
{
  std::vector<std::string> names;
  std::vector<double> velocities;
};

// The twist command,  frame_id is the name of the frame in which the command is specified in.
// frame_id must always be specified.
struct TwistCommand
{
  std::string frame_id;
  Eigen::Vector<double, 6> velocities;
};

// The Pose command,  frame_id is the name of the frame in which the command is specified in.
// frame_id must always be specified.
struct PoseCommand
{
  std::string frame_id;
  Eigen::Isometry3d pose;
};

// The generic input type for servo that can be JointJog, Twist or Pose.
typedef std::variant<JointJogCommand, TwistCommand, PoseCommand> ServoInput;

// The output datatype of servo, this structure contains the names of the joints along with their positions, velocities and accelerations.
struct KinematicState
{
  std::vector<std::string> joint_names;
  Eigen::VectorXd positions, velocities, accelerations;
  rclcpp::Time time_stamp;

  KinematicState(const int num_joints)
  {
    joint_names.resize(num_joints);
    positions = Eigen::VectorXd::Zero(num_joints);
    velocities = Eigen::VectorXd::Zero(num_joints);
    accelerations = Eigen::VectorXd::Zero(num_joints);
  }

  KinematicState()
  {
  }
};

// Mapping joint names and their position in the move group vector
typedef std::unordered_map<std::string, std::size_t> JointNameToMoveGroupIndexMap;

}  // namespace moveit_servo