Program Listing for File utils.hpp

↰ Return to documentation for file (include/moveit/trajectory_cache/utils/utils.hpp)

// Copyright 2024 Intrinsic Innovation LLC.
//
// 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 <string>
#include <vector>

#include <rclcpp/version.h>

#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/srv/get_cartesian_path.hpp>

// For Rolling, Kilted, and newer
#if RCLCPP_VERSION_GTE(29, 6, 0)
#include <tf2_ros/buffer.hpp>
// For Jazzy and older
#else
#include <tf2_ros/buffer.h>
#endif

#include <warehouse_ros/message_collection.h>

namespace moveit_ros
{
namespace trajectory_cache
{

// Frames. =========================================================================================

std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
                                const moveit_msgs::msg::WorkspaceParameters& workspace_parameters);

std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
                                           const moveit_msgs::srv::GetCartesianPath::Request& path_request);

moveit::core::MoveItErrorCode restateInNewFrame(const std::shared_ptr<tf2_ros::Buffer>& tf,
                                                const std::string& target_frame, const std::string& source_frame,
                                                geometry_msgs::msg::Point* translation,
                                                geometry_msgs::msg::Quaternion* rotation,
                                                const tf2::TimePoint& lookup_time = tf2::TimePointZero);

// Execution Time. =================================================================================

double getExecutionTime(const moveit_msgs::msg::RobotTrajectory& trajectory);

// Request Construction. ===========================================================================

moveit_msgs::srv::GetCartesianPath::Request
constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group,
                                 const std::vector<geometry_msgs::msg::Pose>& waypoints, double max_step,
                                 double jump_threshold, bool avoid_collisions = true);

// Queries. ========================================================================================

void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center,
                                    double tolerance);

// Constraints. ====================================================================================

void sortJointConstraints(std::vector<moveit_msgs::msg::JointConstraint>& joint_constraints);

void sortPositionConstraints(std::vector<moveit_msgs::msg::PositionConstraint>& position_constraints);

void sortOrientationConstraints(std::vector<moveit_msgs::msg::OrientationConstraint>& orientation_constraints);

moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance(
    warehouse_ros::Query& query, std::vector<moveit_msgs::msg::Constraints> constraints,
    const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance,
    const std::string& reference_frame_id, const std::string& prefix);

moveit::core::MoveItErrorCode
appendConstraintsAsInsertMetadata(warehouse_ros::Metadata& metadata,
                                  std::vector<moveit_msgs::msg::Constraints> constraints,
                                  const moveit::planning_interface::MoveGroupInterface& move_group,
                                  const std::string& reference_frame_id, const std::string& prefix);

// RobotState. =====================================================================================

moveit::core::MoveItErrorCode
appendRobotStateJointStateAsFetchQueryWithTolerance(warehouse_ros::Query& query,
                                                    const moveit_msgs::msg::RobotState& robot_state,
                                                    const moveit::planning_interface::MoveGroupInterface& move_group,
                                                    double match_tolerance, const std::string& prefix);

moveit::core::MoveItErrorCode appendRobotStateJointStateAsInsertMetadata(
    warehouse_ros::Metadata& metadata, const moveit_msgs::msg::RobotState& robot_state,
    const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& prefix);

}  // namespace trajectory_cache
}  // namespace moveit_ros