moveit_msgs/RobotTrajectory 转换moveit_msgs/DisplayTra，moveit::core/RobotState转换moveit_msgs/RobotState

_无往而不胜_

已于 2023-07-21 10:59:06 修改

阅读量414

点赞数

分类专栏： ros 文章标签： moveit_msgs moveit robotTrajectory displayTrajecto robotState

于 2023-07-20 18:44:49 首次发布

本文链接：https://blog.csdn.net/qq_15204179/article/details/131837390

版权

ros 专栏收录该内容

87 篇文章 19 订阅

订阅专栏

本文展示了如何在ROS环境下，使用MoveIt!库将moveit_msgs::RobotTrajectory转换为moveit_msgs::RobotState以及moveit::core::RobotState。代码示例中，首先初始化MoveGroup接口，然后获取当前机器人状态，并进行转换。之后，可以对转换后的RobotState进行修改，最后发布转换结果并执行规划的运动路径。

摘要由CSDN通过智能技术生成

moveit_msgs/RobotTrajectory 转换成moveit_msgs/RobotState，moveit::core::RobotState转换moveit_msgs::RobotState

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit_msgs/DisplayTrajectory.h>


#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/RobotState.h>
moveit_msgs::RobotState robot_state_msg; //当前机器人状态
moveit_msgs::DisplayTrajectory convertToDisplayTrajectory(const moveit_msgs::RobotTrajectory& robot_trajectory)
{

    moveit_msgs::DisplayTrajectory display_trajectory;
    display_trajectory.trajectory_start=robot_state_msg;
    display_trajectory.trajectory.push_back(robot_trajectory);
    return display_trajectory;
}
// rosrun robot_ctrl   test_6_node
int main(int argc, char** argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "test_6_node");
    ros::NodeHandle node_handle;

    ros::AsyncSpinner spinner(1);
    spinner.start();
    //  ros::Subscriber trajectory_topic_sub_; 接收轨迹并显示  ；trajectory_visualization.cpp 71   trajectory_topic_property_ =new rviz::RosTopicProperty("Trajectory Topic", "move_group/display_planned_path",
    //  trajectory_topic_sub_ = update_nh_.subscribe(trajectory_topic_property_->getStdString(), 2,&TrajectoryVisualization::incomingDisplayTrajectory, this); //void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::DisplayTrajectory::ConstPtr& msg)
    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

    // 初始化MoveGroup接口
    moveit::planning_interface::MoveGroupInterface move_group("manipulator");
   // 获取当前机器人状态
    moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
    // 设置目标关节角度（弧度）
   // std::vector<double> target_joint_values = {0.0, -0.785, 0.785, -1.571, 0.0, 0.0};
    std::vector<double> target_joint_values = {0.0, -0.785, 0.785, -1.571, 0.0, 0.0};
    // 移动机器人到目标关节角度
    move_group.setJointValueTarget(target_joint_values);
    //----------------------moveit_msgs::RobotState 与 moveit::core::RobotState 互相转换 ------------------------------------------------------
    moveit_msgs::RobotState robot_state_msg; //当前机器人状态
    // 将当前机器人状态 moveit::core::RobotStatePtr 转换为moveit_msgs::RobotState
   // moveit_msgs::RobotState robot_state_msg;
    moveit::core::robotStateToRobotStateMsg(*current_state, robot_state_msg);//#include <moveit/robot_state/conversions.h>
    //  moveit_msgs::RobotState 转换为 将当前机器人状态 moveit::core::RobotStatePtr
    //moveit::core::RobotState start_state(move_group.getRobotModel());
    //moveit::core::robotStateMsgToRobotState(robot_state_msg, start_state, true);//将current_plan_->start_state_中的机器人状态消息转换为start_state的机器人状态对象。该函数用于将机器人状态消息转换为可操作的机器人状态对象。
    //-------------------------------------------------------------------------------
   //  可选：修改机器人状态数据 robot_state_msg
   //  TODO: 在这里进行机器人状态数据的修改，比如设置关节角度或位置

   //  发布moveit_msgs::RobotState数据到话题
    ros::Publisher robot_state_pub = node_handle.advertise<moveit_msgs::RobotState>("/my_robot_state", 1, true);
    robot_state_pub.publish(robot_state_msg);


    // 输出当前机器人状态
    ROS_INFO_STREAM("Current Robot State:\n" << robot_state_msg);
    // 规划运动
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;

    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

    if (success)
    {
        ROS_INFO("规划成功！");
        moveit_msgs::RobotTrajectory trajectory_msgs=  my_plan.trajectory_;
        moveit_msgs::DisplayTrajectory display_trajectory = convertToDisplayTrajectory(trajectory_msgs);
        // 显示规划的路径
        display_publisher.publish(display_trajectory);

        // 等待一段时间，以便查看规划路径
        ros::Duration(5.0).sleep();

        ROS_INFO("执行中...");
        // 执行运动
        move_group.execute(my_plan);
        ROS_INFO("运动完成！");
    }
    else
    {
        ROS_ERROR("规划失败！");
    }
    ros::waitForShutdown();
    //ros::shutdown();
    return 0;
}