cp_trajectory_executor.hpp

Go to the documentation of this file.

// Copyright 2021 RobosoftAI Inc.
//
// 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.
 
/*****************************************************************************************************************
 *
 *   Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 *****************************************************************************************************************/
 
#pragma once
 
#include <smacc2/component.hpp>
 
#include <cl_moveit2z/cl_moveit2z.hpp>
#include <cl_moveit2z/components/cp_trajectory_history.hpp>
 
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
 
#include <chrono>
#include <optional>
#include <string>
 
namespace cl_moveit2z
{
struct ExecutionOptions
{
  std::optional<double> maxVelocityScaling;
  std::optional<double> maxAccelerationScaling;
  bool recordToHistory = true;
  std::optional<std::string> trajectoryName;
};
 
struct ExecutionResult
{
  bool success;
  moveit_msgs::msg::MoveItErrorCodes errorCode;
  std::string errorMessage;
  std::chrono::duration<double> executionTime;
 
  ExecutionResult() : success(false), errorCode(), errorMessage(""), executionTime(0.0)
  {
    errorCode.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
  }
};
 
class CpTrajectoryExecutor : public smacc2::ISmaccComponent
{
public:
  CpTrajectoryExecutor() = default;
  virtual ~CpTrajectoryExecutor() = default;
 
  inline void onInitialize() override
  {
    this->requiresClient(moveit2zClient_);
 
    // CpTrajectoryHistory is optional but recommended
    this->requiresComponent(trajectoryHistory_, false);
 
    if (trajectoryHistory_)
    {
      RCLCPP_INFO(
        getLogger(),
        "[CpTrajectoryExecutor] Component initialized with trajectory history recording enabled");
    }
    else
    {
      RCLCPP_WARN(
        getLogger(),
        "[CpTrajectoryExecutor] Component initialized without CpTrajectoryHistory "
        "(trajectory history will not be recorded)");
    }
  }
 
  inline ExecutionResult execute(
    const moveit_msgs::msg::RobotTrajectory & trajectory, const ExecutionOptions & options = {})
  {
    ExecutionResult result;
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
      return result;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      // Apply execution options
      applyExecutionOptions(*moveGroup, options);
 
      RCLCPP_INFO(
        getLogger(), "[CpTrajectoryExecutor] Executing trajectory with %lu points...",
        trajectory.joint_trajectory.points.size());
 
      // Track execution time
      auto startTime = std::chrono::steady_clock::now();
 
      // Execute trajectory
      result.errorCode = moveGroup->execute(trajectory);
 
      auto endTime = std::chrono::steady_clock::now();
      result.executionTime = std::chrono::duration<double>(endTime - startTime);
 
      result.success = (result.errorCode.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
 
      if (result.success)
      {
        RCLCPP_INFO(
          getLogger(), "[CpTrajectoryExecutor] Trajectory execution succeeded (%.2f seconds)",
          result.executionTime.count());
      }
      else
      {
        result.errorMessage =
          "Trajectory execution failed with error code: " + std::to_string(result.errorCode.val);
        RCLCPP_WARN(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
      }
 
      // Record to history if enabled
      if (options.recordToHistory && trajectoryHistory_)
      {
        std::string trajName = options.trajectoryName.value_or("unnamed_trajectory");
        recordToHistory(trajectory, result.errorCode, trajName);
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during trajectory execution: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] %s", result.errorMessage.c_str());
    }
 
    return result;
  }
 
  inline ExecutionResult executePlan(
    const moveit::planning_interface::MoveGroupInterface::Plan & plan,
    const ExecutionOptions & options = {})
  {
    return execute(plan.trajectory_, options);
  }
 
  inline void cancel()
  {
    if (!moveit2zClient_)
    {
      RCLCPP_ERROR(getLogger(), "[CpTrajectoryExecutor] Cannot cancel: client not available");
      return;
    }
 
    auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
    try
    {
      moveGroup->stop();
      RCLCPP_INFO(getLogger(), "[CpTrajectoryExecutor] Trajectory execution cancelled");
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR(
        getLogger(), "[CpTrajectoryExecutor] Exception during cancellation: %s", e.what());
    }
  }
 
  inline CpTrajectoryHistory * getTrajectoryHistory() { return trajectoryHistory_; }
 
private:
  ClMoveit2z * moveit2zClient_ = nullptr;
  CpTrajectoryHistory * trajectoryHistory_ = nullptr;
 
  inline void applyExecutionOptions(
    moveit::planning_interface::MoveGroupInterface & moveGroup, const ExecutionOptions & options)
  {
    if (options.maxVelocityScaling)
    {
      moveGroup.setMaxVelocityScalingFactor(*options.maxVelocityScaling);
    }
 
    if (options.maxAccelerationScaling)
    {
      moveGroup.setMaxAccelerationScalingFactor(*options.maxAccelerationScaling);
    }
  }
 
  inline void recordToHistory(
    const moveit_msgs::msg::RobotTrajectory & trajectory,
    const moveit_msgs::msg::MoveItErrorCodes & errorCode, const std::string & name)
  {
    if (!trajectoryHistory_)
    {
      return;
    }
 
    try
    {
      trajectoryHistory_->pushTrajectory(name, trajectory, errorCode);
      RCLCPP_DEBUG(
        getLogger(), "[CpTrajectoryExecutor] Recorded trajectory '%s' to history", name.c_str());
    }
    catch (const std::exception & e)
    {
      RCLCPP_WARN(
        getLogger(), "[CpTrajectoryExecutor] Failed to record trajectory to history: %s", e.what());
    }
  }
};
 
}  // namespace cl_moveit2z