Skip to content

Commit

Permalink
Add utils library with test fixtures
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
@methylDragon
methylDragon committed Aug 1, 2024
1 parent ff39939 commit 77a3307
Show file tree
Hide file tree
Showing 12 changed files with 777 additions and 107 deletions.
44 changes: 22 additions & 22 deletions moveit_ros/trajectory_cache/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,24 @@ set(TRAJECTORY_CACHE_DEPENDENCIES

# ==============================================================================

set(TRAJECTORY_CACHE_LIBRARIES moveit_ros_trajectory_cache_utils_lib
moveit_ros_trajectory_cache_lib)

# Utils library
add_library(moveit_ros_trajectory_cache_utils_lib src/utils/utils.cpp)
generate_export_header(moveit_ros_trajectory_cache_utils_lib)
target_include_directories(
moveit_ros_trajectory_cache_utils_lib
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib
${TRAJECTORY_CACHE_DEPENDENCIES})

# Trajectory cache library
add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp)
generate_export_header(moveit_ros_trajectory_cache_lib)
target_link_libraries(moveit_ros_trajectory_cache_lib
moveit_ros_trajectory_cache_utils_lib)
target_include_directories(
moveit_ros_trajectory_cache_lib
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
Expand All @@ -43,7 +58,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib
${TRAJECTORY_CACHE_DEPENDENCIES})

install(
TARGETS moveit_ros_trajectory_cache_lib
TARGETS ${TRAJECTORY_CACHE_LIBRARIES}
EXPORT moveit_ros_trajectory_cacheTargets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
Expand All @@ -52,29 +67,14 @@ install(
DESTINATION include/moveit_ros_trajectory_cache)

install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache)
install(
FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h
DESTINATION include/moveit_ros_trajectory_cache)

# ==============================================================================

if(BUILD_TESTING)
find_package(ament_cmake_pytest REQUIRED)
find_package(launch_testing_ament_cmake REQUIRED)
find_package(rmf_utils REQUIRED)

# This test executable is run by the pytest_test, since a node is required for
# testing move groups
add_executable(test_trajectory_cache test/test_trajectory_cache.cpp)
target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib)
# Install export headers for each library
foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES})
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h
DESTINATION include/moveit_ros_trajectory_cache)
endforeach()

install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME})

ament_add_pytest_test(
test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY
"${CMAKE_CURRENT_BINARY_DIR}")

endif()
add_subdirectory(test)

ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET)
ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES})
Expand Down
59 changes: 59 additions & 0 deletions moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
// 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.

/** @file
* @brief Utilities used by the trajectory_cache package.
* @author methylDragon
*/

#pragma once

#include <string>
#include <vector>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/srv/get_cartesian_path.hpp>

#include <warehouse_ros/message_collection.h>

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

/** @brief Gets workspace frame ID.
* If workspace_parameters has no frame ID, fetch it from move_group.
*/
std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::WorkspaceParameters& workspace_parameters);

/** @brief Gets cartesian path request frame ID.
* If path_request has no frame ID, fetch it from move_group.
*/
std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::srv::GetCartesianPath::Request& path_request);

// Features. =======================================================================================

/** @brief Appends a range inclusive query with some tolerance about some center value. */
void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center,
double tolerance);

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

/** @brief Sorts a vector of joint constraints in-place by joint name. */
void sortJointConstraints(std::vector<moveit_msgs::msg::JointConstraint>& joint_constraints);

/** @brief Sorts a vector of position constraints in-place by link name. */
void sortPositionConstraints(std::vector<moveit_msgs::msg::PositionConstraint>& position_constraints);

/** @brief Sorts a vector of orientation constraints in-place by link name. */
void sortOrientationConstraints(std::vector<moveit_msgs::msg::OrientationConstraint>& orientation_constraints);
118 changes: 33 additions & 85 deletions moveit_ros/trajectory_cache/src/trajectory_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logging.hpp>
#include <moveit/trajectory_cache/trajectory_cache.hpp>
#include <moveit/trajectory_cache/utils/utils.hpp>

namespace moveit_ros
{
Expand All @@ -38,63 +39,6 @@ using warehouse_ros::MessageWithMetadata;
using warehouse_ros::Metadata;
using warehouse_ros::Query;

// Utils =======================================================================

// Ensure we always have a workspace frame ID.
std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::WorkspaceParameters& workspace_parameters)
{
if (workspace_parameters.header.frame_id.empty())
{
return move_group.getRobotModel()->getModelFrame();
}
else
{
return workspace_parameters.header.frame_id;
}
}

// Ensure we always have a cartesian path request frame ID.
std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::srv::GetCartesianPath::Request& path_request)
{
if (path_request.header.frame_id.empty())
{
return move_group.getPoseReferenceFrame();
}
else
{
return path_request.header.frame_id;
}
}

// Append a range inclusive query with some tolerance about some center value.
void queryAppendRangeInclusiveWithTolerance(Query& query, const std::string& name, double center, double tolerance)
{
query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2);
}

// Sort constraint components by joint or link name.
void sortConstraints(std::vector<moveit_msgs::msg::JointConstraint>& joint_constraints,
std::vector<moveit_msgs::msg::PositionConstraint>& position_constraints,
std::vector<moveit_msgs::msg::OrientationConstraint>& orientation_constraints)
{
std::sort(joint_constraints.begin(), joint_constraints.end(),
[](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) {
return l.joint_name < r.joint_name;
});

std::sort(position_constraints.begin(), position_constraints.end(),
[](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) {
return l.link_name < r.link_name;
});

std::sort(orientation_constraints.begin(), orientation_constraints.end(),
[](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) {
return l.link_name < r.link_name;
});
}

// Trajectory Cache ============================================================

TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node)
Expand Down Expand Up @@ -595,7 +539,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery(
for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++)
{
query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i));
queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
current_state_msg.joint_state.position.at(i), match_tolerance);
}
}
Expand All @@ -604,7 +548,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery(
for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++)
{
query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i));
queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
plan_request.start_state.joint_state.position.at(i), match_tolerance);
}
}
Expand Down Expand Up @@ -641,11 +585,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery(
"Not supported.");
}

queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor,
queryAppendCenterWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor,
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor",
queryAppendCenterWithTolerance(query, "max_acceleration_scaling_factor",
plan_request.max_acceleration_scaling_factor, match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed,
queryAppendCenterWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed,
match_tolerance);

// Extract constraints (so we don't have cardinality on goal_constraint idx.)
Expand All @@ -668,7 +612,9 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery(
}

// Also sort for even less cardinality.
sortConstraints(joint_constraints, position_constraints, orientation_constraints);
sortJointConstraints(joint_constraints);
sortPositionConstraints(position_constraints);
sortOrientationConstraints(orientation_constraints);
}

// Joint constraints
Expand All @@ -678,7 +624,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery(
std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++);

query.append(meta_name + ".joint_name", constraint.joint_name);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance);
queryAppendCenterWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance);
query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above);
query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below);
}
Expand Down Expand Up @@ -727,11 +673,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery(

query.append(meta_name + ".link_name", constraint.link_name);

queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x",
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.x",
x_offset + constraint.target_point_offset.x, match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y",
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.y",
y_offset + constraint.target_point_offset.y, match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z",
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.z",
z_offset + constraint.target_point_offset.z, match_tolerance);
}
}
Expand Down Expand Up @@ -794,13 +740,13 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery(
auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset;
final_quat.normalize();

queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(),
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(),
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(),
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(),
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(),
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(),
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(),
queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(),
match_tolerance);
}
}
Expand Down Expand Up @@ -942,7 +888,9 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata(
}

// Also sort for even less cardinality.
sortConstraints(joint_constraints, position_constraints, orientation_constraints);
sortJointConstraints(joint_constraints);
sortPositionConstraints(position_constraints);
sortOrientationConstraints(orientation_constraints);
}

// Joint constraints
Expand Down Expand Up @@ -1135,7 +1083,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery(
for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++)
{
query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i));
queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
current_state_msg.joint_state.position.at(i), match_tolerance);
}
}
Expand All @@ -1144,7 +1092,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery(
for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++)
{
query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i));
queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i),
plan_request.start_state.joint_state.position.at(i), match_tolerance);
}
}
Expand Down Expand Up @@ -1172,12 +1120,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery(
RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported.");
}

queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor,
queryAppendCenterWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor,
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor",
queryAppendCenterWithTolerance(query, "max_acceleration_scaling_factor",
plan_request.max_acceleration_scaling_factor, match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, "max_step", plan_request.max_step, match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance);
queryAppendCenterWithTolerance(query, "max_step", plan_request.max_step, match_tolerance);
queryAppendCenterWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance);

// Waypoints
// Restating them in terms of the robot model frame (usually base_link)
Expand Down Expand Up @@ -1229,11 +1177,11 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery(

// Apply offsets
// Position
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x,
queryAppendCenterWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x,
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y,
queryAppendCenterWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y,
match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z,
queryAppendCenterWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z,
match_tolerance);

// Orientation
Expand All @@ -1244,10 +1192,10 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery(
auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset;
final_quat.normalize();

queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance);
queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance);
queryAppendCenterWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance);
queryAppendCenterWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance);
queryAppendCenterWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance);
queryAppendCenterWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance);
}

query.append("link_name", plan_request.link_name);
Expand Down
Loading

0 comments on commit 77a3307

Please sign in to comment.