kinova test_accuracy源码解析

最新推荐文章于 2025-06-12 10:07:18 发布

原创

最新推荐文章于 2025-06-12 10:07:18 发布 · 816 阅读

1 ·

CC 4.0 BY-SA版权

void PickPlace::evaluate_move_accuracy()
{
    robot_state::RobotState target_state =	group_->getJointValueTarget();
    target_state.update();
    //group_->getcurrentstate does not give the correct values
    robot_state::RobotState current_state(target_state);
    //geting state from joint state message
    std::string topic;
    if (robot_connected_)
    {
        topic = "/" + robot_type_ + "_driver/out/joint_state";
    }
    else
    {
        topic = "/" + robot_type_ + "/joint_states"; //for gazebo
    }
    sensor_msgs::JointStateConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(topic, ros::Duration(1.0));
    for (int i =0;i<joint_names_.size();i++)
    {
        current_state.setJointPositions(joint_names_[i].c_str(), &(msg->position[i]) );
    }
    current_state.update();
    Eigen::Affine3d transform = current_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
    geometry_msgs::Pose feeback_pose;
    tf::poseEigenToMsg(transform,feeback_pose);

    transform = target_state.getGlobalLinkTransform (robot_type_ + "_end_effector");
    geometry_msgs::Pose target_state_pose;
    tf::poseEigenToMsg(transform,target_state_pose);


    std::string error_out,joints_out,torques_out;
    error_out.clear();
    joints_out.clear();
    torques_out.clear();
    o_file_<<std::endl<<std::endl<<"**********************New trajectory***************************"<<std::endl;
    //geometry_msgs::PoseStampedConstPtr pose_robot = ros::topic::waitForMessage<geomet