Skip to content

Commit

Permalink
Clean up logging and comments
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
@methylDragon
methylDragon committed Jul 23, 2024
1 parent 3cbfdcc commit cf67313
Show file tree
Hide file tree
Showing 3 changed files with 33 additions and 48 deletions.
7 changes: 2 additions & 5 deletions moveit_ros/trajectory_cache/include/moveit/trajectory_cache/trajectory_cache.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SRC__TRAJECTORY_CACHE_HPP
#define SRC__TRAJECTORY_CACHE_HPP
#pragma once

#include <chrono>
#include <memory>
Expand Down Expand Up @@ -256,6 +255,4 @@ class TrajectoryCache
};

} // namespace trajectory_cache
} // namespace moveit_ros

#endif // SRC__TRAJECTORY_CACHE_HPP
} // namespace moveit_ros
70 changes: 29 additions & 41 deletions moveit_ros/trajectory_cache/src/trajectory_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) : node_(no

bool TrajectoryCache::init(const std::string& db_path, uint32_t db_port, double exact_match_precision)
{
RCLCPP_INFO(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)",
RCLCPP_DEBUG(node_->get_logger(), "Opening trajectory cache database at: %s (Port: %d, Precision: %f)",
db_path.c_str(), db_port, exact_match_precision);

// If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros'
Expand Down Expand Up @@ -138,7 +138,7 @@ MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr TrajectoryCache

if (matching_trajectories.empty())
{
RCLCPP_INFO(node_->get_logger(), "No matching trajectories found.");
RCLCPP_DEBUG(node_->get_logger(), "No matching trajectories found.");
return nullptr;
}

Expand Down Expand Up @@ -211,7 +211,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup
if (execution_time_s < match_execution_time_s)
{
int delete_id = match->lookupInt("id");
RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Overwriting plan (id: %d): "
"execution_time (%es) > new trajectory's execution_time (%es)",
delete_id, match_execution_time_s, execution_time_s);
Expand Down Expand Up @@ -242,7 +242,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup
return false;
}

RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Inserting trajectory: New trajectory execution_time (%es) "
"is better than best trajectory's execution_time (%es)",
execution_time_s, best_execution_time);
Expand All @@ -251,7 +251,7 @@ bool TrajectoryCache::put_trajectory(const moveit::planning_interface::MoveGroup
return true;
}

RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Skipping plan insert: New trajectory execution_time (%es) "
"is worse than best trajectory's execution_time (%es)",
execution_time_s, best_execution_time);
Expand All @@ -275,8 +275,6 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query(
RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported.");
}

// auto original = *query; // Copy not supported.

query.append("group_name", plan_request.group_name);

// Workspace params
Expand Down Expand Up @@ -311,7 +309,8 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_query(
if (!current_state)
{
RCLCPP_WARN(node_->get_logger(), "Skipping start query append: Could not get robot state.");
// *query = original; // Undo our changes. (Can't. Copy not supported.)
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}

Expand Down Expand Up @@ -366,8 +365,6 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query(
"Not supported.");
}

// auto original = *query; // Copy not supported.

query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor",
plan_request.max_velocity_scaling_factor, match_tolerance);
query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor",
Expand Down Expand Up @@ -447,8 +444,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query(
plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(),
ex.what());

// (Can't. Copy not supported.)
// *query = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down Expand Up @@ -501,8 +498,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_query(
plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(),
ex.what());

// (Can't. Copy not supported.)
// *query = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down Expand Up @@ -552,8 +549,6 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata(
RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported.");
}

// auto original = *metadata; // Copy not supported.

metadata.append("group_name", plan_request.group_name);

// Workspace params
Expand Down Expand Up @@ -587,7 +582,8 @@ bool TrajectoryCache::extract_and_append_trajectory_start_to_metadata(
if (!current_state)
{
RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state.");
// *metadata = original; // Undo our changes. // Copy not supported
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}

Expand Down Expand Up @@ -642,8 +638,6 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata(
"Not supported.");
}

// auto original = *metadata; // Copy not supported.

metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor);
metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor);
metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed);
Expand Down Expand Up @@ -720,8 +714,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata(
plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(),
ex.what());

// (Can't. Copy not supported.)
// *metadata = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down Expand Up @@ -771,8 +765,8 @@ bool TrajectoryCache::extract_and_append_trajectory_goal_to_metadata(
plan_request.workspace_parameters.header.frame_id.c_str(), constraint.header.frame_id.c_str(),
ex.what());

// (Can't. Copy not supported.)
// *metadata = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down Expand Up @@ -879,7 +873,7 @@ TrajectoryCache::fetch_best_matching_cartesian_trajectory(

if (matching_trajectories.empty())
{
RCLCPP_INFO(node_->get_logger(), "No matching cartesian trajectories found.");
RCLCPP_DEBUG(node_->get_logger(), "No matching cartesian trajectories found.");
return nullptr;
}

Expand Down Expand Up @@ -947,7 +941,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface:
if (execution_time_s < match_execution_time_s)
{
int delete_id = match->lookupInt("id");
RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Overwriting cartesian trajectory (id: %d): "
"execution_time (%es) > new trajectory's execution_time (%es)",
delete_id, match_execution_time_s, execution_time_s);
Expand Down Expand Up @@ -980,7 +974,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface:
return false;
}

RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Inserting cartesian trajectory: New trajectory execution_time (%es) "
"is better than best trajectory's execution_time (%es) at fraction (%es)",
execution_time_s, best_execution_time, fraction);
Expand All @@ -989,7 +983,7 @@ bool TrajectoryCache::put_cartesian_trajectory(const moveit::planning_interface:
return true;
}

RCLCPP_INFO(node_->get_logger(),
RCLCPP_DEBUG(node_->get_logger(),
"Skipping cartesian trajectory insert: New trajectory execution_time (%es) "
"is worse than best trajectory's execution_time (%es) at fraction (%es)",
execution_time_s, best_execution_time, fraction);
Expand All @@ -1013,8 +1007,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query(
RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported.");
}

// auto original = *metadata; // Copy not supported.

query.append("group_name", plan_request.group_name);

// Joint state
Expand All @@ -1039,7 +1031,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_query(
if (!current_state)
{
RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state.");
// *metadata = original; // Undo our changes. // Copy not supported
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}

Expand Down Expand Up @@ -1085,8 +1078,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query(
RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported.");
}

// auto original = *metadata; // Copy not supported.

query_append_range_inclusive_with_tolerance(query, "max_velocity_scaling_factor",
plan_request.max_velocity_scaling_factor, match_tolerance);
query_append_range_inclusive_with_tolerance(query, "max_acceleration_scaling_factor",
Expand Down Expand Up @@ -1127,8 +1118,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_query(
"Could not get goal transform for %s to %s: %s",
base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what());

// (Can't. Copy not supported.)
// *metadata = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down Expand Up @@ -1185,8 +1176,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata(
RCLCPP_WARN(node_->get_logger(), "Ignoring start_state.attached_collision_objects: Not supported.");
}

// auto original = *metadata; // Copy not supported.

metadata.append("group_name", plan_request.group_name);

// Joint state
Expand All @@ -1211,7 +1200,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_start_to_metadata(
if (!current_state)
{
RCLCPP_WARN(node_->get_logger(), "Skipping start metadata append: Could not get robot state.");
// *metadata = original; // Undo our changes. // Copy not supported
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}

Expand Down Expand Up @@ -1256,8 +1246,6 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata(
RCLCPP_WARN(node_->get_logger(), "Ignoring avoid_collisions: Not supported.");
}

// auto original = *metadata; // Copy not supported.

metadata.append("max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor);
metadata.append("max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor);
metadata.append("max_step", plan_request.max_step);
Expand Down Expand Up @@ -1296,8 +1284,8 @@ bool TrajectoryCache::extract_and_append_cartesian_trajectory_goal_to_metadata(
"Could not get goal transform for %s to %s: %s",
base_frame.c_str(), plan_request.header.frame_id.c_str(), ex.what());

// (Can't. Copy not supported.)
// *metadata = original; // Undo our changes.
// NOTE: methyldragon -
// Ideally we would restore the original state here and undo our changes, however copy of the query is not supported.
return false;
}
}
Expand Down
4 changes: 2 additions & 2 deletions moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ std::vector<geometry_msgs::msg::Pose> get_dummy_waypoints()
void test_motion_trajectories(std::shared_ptr<MoveGroupInterface> move_group, std::shared_ptr<TrajectoryCache> cache)
{
// Setup =====================================================================
// All variants are copies.
// All variants are modified copies of `plan_req`.

/// MotionPlanRequest

Expand Down Expand Up @@ -577,7 +577,7 @@ void test_cartesian_trajectories(std::shared_ptr<MoveGroupInterface> move_group,
prefix, "Ok");

// Setup =====================================================================
// All variants are copies.
// All variants are modified copies of `cartesian_plan_req`.

/// GetCartesianPath::Request

Expand Down

0 comments on commit cf67313

Please sign in to comment.