机器人 关于空间规划由python代码转换到C++代码

机器人 关于空间规划由python代码转换到C++代码

flyfish

ROS
原文的空间规划是这样的
moveit_fk_demo.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)

        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')

        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = moveit_commander.MoveGroupCommander('gripper')

        # 设置机械臂和夹爪的允许误差值
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.001)

        # 控制机械臂先回到初始化位置
        arm.set_named_target('home')
        arm.go()
        rospy.sleep(2)

        # 设置夹爪的目标位置，并控制夹爪运动
        gripper.set_joint_value_target([0.01])
        gripper.go()
        rospy.sleep(1)

        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
        arm.set_joint_value_target(joint_positions)

        # 控制机械臂完成运动
        arm.go()
        rospy.sleep(1)

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

经过C++转换是这样的 名字以Test为例 主意大小写敏感

#include 
// 包含moveit的API
#include 
#include 
#include 
#include 
#include 
#include 
#include 
int main(int argc, char **argv)
{
    ros::init (argc, argv, "moveit_fk_demo",ros::init_options::AnonymousName);
    ros::AsyncSpinner spinner(1);
    spinner.start();

    //初始化需要使用move group控制的机械臂中的arm group
    moveit::planning_interface::MoveGroupInterface arm("arm");

    //初始化需要使用move group控制的机械臂中的gripper group
    moveit::planning_interface::MoveGroupInterface gripper("gripper");



    //设置机械臂和夹爪的允许误差值
    arm.setGoalJointTolerance(0.001);
    gripper.setGoalJointTolerance(0.001);


    //控制机械臂先回到初始化位置
    arm.setNamedTarget("home");
    arm.move();
    sleep(2);

    //设置夹爪的目标位置，并控制夹爪运动
    std::vector<double> gripper_positions;
    gripper_positions.push_back(0.01);
    gripper.setJointValueTarget(gripper_positions);
    gripper.move();
    sleep(1);

    //机械臂随机目标位置
    arm.setRandomTarget();
    // 开始运动规划，并且让机械臂移动到目标位置
    arm.move();
    sleep(1);

    //设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
    std::vector<double> arm_positions;
    arm_positions.push_back(-0.0867);
    arm_positions.push_back(-1.274);
    arm_positions.push_back(0.02832);
    arm_positions.push_back(0.0820);
    arm_positions.push_back(-1.273);
    arm_positions.push_back(-0.003);
    arm.setJointValueTarget(arm_positions);
    // # 控制机械臂完成运动
    arm.move();
    sleep(1);

    ros::shutdown();

}

//

CMakeLists.txt 增加

add_executable(Test
  src/Test.cpp
)
add_dependencies(Test ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(Test
  ${catkin_LIBRARIES}
)