MoveIt 一个由于线程设置错误导致的问题

编写了一个通过话题接收Pose,并以此Pose作为posetarget来使用moveit

在主函数中,不应该将线程数设置为1。
在设置为1时,程序执行到规划处 (move_group.plan(my_plan) ) 就不再继续运行bool 及其后面的语句,且不报错。
(此时在RVIZ内可以看到执行到指定点)

在第一次出现Ready to take commands for planning group panda_arm. 时,节点就不再继续运行。

设置线程数为2时,顺利执行。
这里写图片描述

以下是一个基于MoveIt Simple Controller Manager的ROS控制器示例: ```cpp #include <ros/ros.h> #include <moveit_simple_controller_manager/simple_controller_manager.h> #include <moveit/move_group_interface/move_group_interface.h> #include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/robot_model/robot_model.h> #include <moveit/robot_state/robot_state.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h> class MyRobotController : public moveit_simple_controller_manager::SimpleControllerManager { public: MyRobotController() : moveit_simple_controller_manager::SimpleControllerManager() { // Initialize MoveIt move_group_.reset(new moveit::planning_interface::MoveGroupInterface("arm")); // Load the robot model and get the robot state robot_model_loader_.reset(new robot_model_loader::RobotModelLoader("robot_description")); robot_model_ = robot_model_loader_->getModel(); robot_state_.reset(new robot_state::RobotState(robot_model_)); // Set the joint model group joint_model_group_ = robot_model_->getJointModelGroup("arm"); // Set the trajectory processing trajectory_processing_.reset(new trajectory_processing::IterativeParabolicTimeParameterization()); // Set the desired joint state desired_joint_state_.resize(joint_model_group_->getVariableCount()); desired_joint_state_.setZero(); } virtual bool initialize(const std::string& robot_description, const std::string& controller_name) { // Initialize the controller with the robot description and the controller name if (!SimpleControllerManager::initialize(robot_description, controller_name)) { ROS_ERROR("Failed to initialize the controller"); return false; } // Set the callback function for the controller controller_->setTrajectoryExecutionCallback(boost::bind(&MyRobotController::trajectoryExecutionCallback, this, _1)); // Print a message to indicate successful initialization ROS_INFO("Controller initialized successfully"); return true; } virtual bool canSwitch(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers) const { // This controller does not support switching return false; } virtual bool switchControllers(const std::vector<std::string>& start_controllers, const std::vector<std::string>& stop_controllers) { // This controller does not support switching return false; } virtual bool isActive() { // Return true if the controller is active return controller_->isActive(); } virtual void stopping() { // Stop the controller controller_->stop(); } void trajectoryExecutionCallback(const moveit_msgs::RobotTrajectory& trajectory) { // Get the start state of the robot robot_state_->setToDefaultValues(); const robot_state::JointModelGroup* start_joint_model_group = robot_state_->getJointModelGroup("arm"); // Get the start and end time of the trajectory double start_time = ros::Time::now().toSec(); double end_time = start_time + trajectory.joint_trajectory.points.back().time_from_start.toSec(); // Execute the trajectory move_group_->execute(trajectory); // Wait until the end of the trajectory while (ros::Time::now().toSec() < end_time) { ros::Duration(0.01).sleep(); } // Get the current joint state of the robot robot_state_->getCurrentState(*move_group_->getCurrentState()); const robot_state::JointModelGroup* current_joint_model_group = robot_state_->getJointModelGroup("arm"); // Get the current joint values robot_state_->copyJointGroupPositions(current_joint_model_group, desired_joint_state_); // Set the current joint values as the desired joint state controller_->setDesiredJointState(desired_joint_state_); // Print a message to indicate successful trajectory execution ROS_INFO("Trajectory executed successfully"); } private: ros::NodeHandle nh_; moveit::planning_interface::MoveGroupInterfacePtr move_group_; robot_model_loader::RobotModelLoaderPtr robot_model_loader_; robot_model::RobotModelPtr robot_model_; robot_state::RobotStatePtr robot_state_; const robot_model::JointModelGroup* joint_model_group_; std::unique_ptr<trajectory_processing::IterativeParabolicTimeParameterization> trajectory_processing_; Eigen::VectorXd desired_joint_state_; }; int main(int argc, char** argv) { // Initialize ROS ros::init(argc, argv, "my_robot_controller"); // Create the controller MyRobotController controller; // Initialize the controller if (!controller.initialize("robot_description", "my_robot_controller")) { return -1; } // Spin the node ros::spin(); return 0; } ``` 在上述示例中,MyRobotController是一个自定义的ROS控制器,它继承自MoveIt Simple Controller Manager。在构造函数中,我们初始化了MoveIt,加载了机器人模型,并设置了轨迹处理和期望关节状态。在initialize()函数中,我们初始化了控制器,并设置了回调函数。在trajectoryExecutionCallback()函数中,我们执行了轨迹,并在轨迹执行完成后更新了期望关节状态。最后,在main()函数中,我们创建了并初始化了控制器,并开始运行ROS节点。
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值