114 lines
3.8 KiB
C++
114 lines
3.8 KiB
C++
#include "quadruped_manipulator_control/planning/moveit_planner.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
namespace quadruped_manipulator_control
|
|
{
|
|
|
|
MoveItPlanner::MoveItPlanner(
|
|
const rclcpp::Node::SharedPtr& node,
|
|
const std::string& planning_group)
|
|
: node_(node), planning_group_(planning_group)
|
|
{
|
|
// 初始化机器人模型加载器
|
|
robot_model_loader_ = std::make_shared<robot_model_loader::RobotModelLoader>(node_, "robot_description");
|
|
|
|
// 初始化运动规划组
|
|
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
|
|
|
|
// 设置默认规划参数
|
|
set_planning_parameters();
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "MoveItPlanner initialized for planning group: %s", planning_group_.c_str());
|
|
}
|
|
|
|
void MoveItPlanner::set_planning_group(const std::string& planning_group)
|
|
{
|
|
planning_group_ = planning_group;
|
|
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(node_, planning_group_);
|
|
set_planning_parameters();
|
|
RCLCPP_INFO(node_->get_logger(), "Switched to planning group: %s", planning_group_.c_str());
|
|
}
|
|
|
|
bool MoveItPlanner::plan_joint_goal(
|
|
const std::vector<double>& joint_positions,
|
|
moveit::planning_interface::MoveGroupInterface::Plan& plan)
|
|
{
|
|
if (joint_positions.empty()) {
|
|
RCLCPP_ERROR(node_->get_logger(), "Joint positions vector is empty");
|
|
return false;
|
|
}
|
|
|
|
// 设置关节目标
|
|
move_group_->setJointValueTarget(joint_positions);
|
|
|
|
// 规划运动
|
|
moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
|
|
|
|
if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
|
|
RCLCPP_INFO(node_->get_logger(), "Successfully planned joint space motion");
|
|
return true;
|
|
} else {
|
|
RCLCPP_ERROR(node_->get_logger(), "Failed to plan joint space motion. Error code: %d", result.val);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool MoveItPlanner::plan_pose_goal(
|
|
const geometry_msgs::msg::Pose& target_pose,
|
|
moveit::planning_interface::MoveGroupInterface::Plan& plan)
|
|
{
|
|
// 设置位姿目标
|
|
move_group_->setPoseTarget(target_pose);
|
|
|
|
// 规划运动
|
|
moveit::planning_interface::MoveItErrorCode result = move_group_->plan(plan);
|
|
|
|
if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
|
|
RCLCPP_INFO(node_->get_logger(), "Successfully planned Cartesian space motion");
|
|
return true;
|
|
} else {
|
|
RCLCPP_ERROR(node_->get_logger(), "Failed to plan Cartesian space motion. Error code: %d", result.val);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool MoveItPlanner::execute_plan(const moveit::planning_interface::MoveGroupInterface::Plan& plan)
|
|
{
|
|
moveit::planning_interface::MoveItErrorCode result = move_group_->execute(plan);
|
|
|
|
if (result == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
|
|
RCLCPP_INFO(node_->get_logger(), "Successfully executed planned motion");
|
|
return true;
|
|
} else {
|
|
RCLCPP_ERROR(node_->get_logger(), "Failed to execute planned motion. Error code: %d", result.val);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
std::vector<double> MoveItPlanner::get_current_joint_positions()
|
|
{
|
|
return move_group_->getCurrentJointValues();
|
|
}
|
|
|
|
geometry_msgs::msg::Pose MoveItPlanner::get_current_end_effector_pose()
|
|
{
|
|
return move_group_->getCurrentPose().pose;
|
|
}
|
|
|
|
void MoveItPlanner::set_planning_parameters(
|
|
double planning_time,
|
|
double goal_tolerance,
|
|
double max_velocity_scaling,
|
|
double max_acceleration_scaling)
|
|
{
|
|
move_group_->setPlanningTime(planning_time);
|
|
move_group_->setGoalTolerance(goal_tolerance);
|
|
move_group_->setMaxVelocityScalingFactor(max_velocity_scaling);
|
|
move_group_->setMaxAccelerationScalingFactor(max_acceleration_scaling);
|
|
|
|
RCLCPP_INFO(node_->get_logger(), "Set planning parameters: time=%.2fs, tolerance=%.4fm, velocity=%.2f, acceleration=%.2f",
|
|
planning_time, goal_tolerance, max_velocity_scaling, max_acceleration_scaling);
|
|
}
|
|
|
|
} // namespace quadruped_manipulator_control
|