python - 如何从 python (py) 文件中调用 C++ 函数？

Question

我知道 boost python 库，但我需要的是从 python 文件 (py) 调用 cpp 文件中的 C++ 函数。这是一个移动 PR2 机器人 ARM 的简单 Python 代码。

以下代码在 py 文件中

#!/usr/bin/env python

import sys
import copy
import rospy
import roscpp
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from geometry_msgs.msg import PoseStamped
import tf
import roslib
import traceback


def move_group_python_interface_tutorial():
  ## Initialize moveit commander
  print "============ Starting setup"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('move_group_python_interface_tutorial',
              anonymous=True)

  ## Instantiate a RobotCommander object.  This object is an interface to
  ## the robot as a whole.
  scene = moveit_commander.PlanningSceneInterface()
  robot = moveit_commander.RobotCommander()

  rospy.sleep(2)

  group = moveit_commander.MoveGroupCommander("right_arm")


  display_trajectory_publisher = rospy.Publisher(
                                  '/move_group/display_planned_path',
                                  moveit_msgs.msg.DisplayTrajectory)

  ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
  print "============ Waiting for RVIZ..."
  rospy.sleep(10)

 # I want to call the openGripper() function here

  print "============ Generating plan 1"

  #[ 0.5, -0.5, 0.5, 0.5 ]
  pose_source = geometry_msgs.msg.Pose()
  pose_source.orientation.x = 0.5
  pose_source.orientation.y = 0.5
  pose_source.orientation.z = 0.5
  pose_source.orientation.w = 0.5
  pose_source.position.x = 0.68
  pose_source.position.y = -0.01
  pose_source.position.z = 1.1
  #group.set_planning_time(10);


  group.set_pose_target(pose_source)


  ## Now, we call the planner to compute the plan
  ## and visualize it if successful
  ## Note that we are just planning, not asking move_group 
  ## to actually move the robot

  plan1 = group.plan()

  print "============ Waiting while RVIZ displays plan1..."
  rospy.sleep(5)

  # Uncomment below line when working with a real robot

  group.go(wait=True)

  # I want to call the closedGripper() function here


  group.clear_pose_targets()


  ## When finished shut down moveit_commander.
  moveit_commander.roscpp_shutdown()

  print "============ STOPPING"


if __name__=='__main__':
  try:
    move_group_python_interface_tutorial()
  except rospy.ROSInterruptException:
    pass

这是我在 cpp 文件中的 c++ 代码

#include <ros/ros.h>

// MoveIt!
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/move_group_interface/move_group.h>
#include <geometric_shapes/solid_primitive_dims.h>
#include <string>
#include <unistd.h>
//#include <vector>

//static const std::string ROBOT_DESCRIPTION="robot_description";

opGripper(){
  //std::vector<moveit_msgs::Grasp> grasps;
  moveit_msgs::Grasp g;
  g.pre_grasp_approach.direction.vector.x = 1.0;
  g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link";
  g.pre_grasp_approach.min_distance = 0.2;
  g.pre_grasp_approach.desired_distance = 0.4;

  g.post_grasp_retreat.direction.header.frame_id = "base_footprint";
  g.post_grasp_retreat.direction.vector.z = 1.0;
  g.post_grasp_retreat.min_distance = 0.1;
  g.post_grasp_retreat.desired_distance = 0.25;
  openGripper(g.pre_grasp_posture);
}

closGripper(){

  moveit_msgs::Grasp h;
  h.pre_place_approach.direction.vector.z = -1.0;
  h.post_place_retreat.direction.vector.x = -1.0;
  h.post_place_retreat.direction.header.frame_id = "base_footprint";
  h.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link";
  h.pre_place_approach.min_distance = 0.1;
  h.pre_place_approach.desired_distance = 0.2;
  h.post_place_retreat.min_distance = 0.1;
  h.post_place_retreat.desired_distance = 0.25;


  closedGripper(g.grasp_posture);
}


void openGripper(trajectory_msgs::JointTrajectory& posture){

  posture.joint_names.resize(6);
  posture.joint_names[0] = "r_gripper_joint";
  posture.joint_names[1] = "r_gripper_motor_screw_joint";
  posture.joint_names[2] = "r_gripper_l_finger_joint";
  posture.joint_names[3] = "r_gripper_r_finger_joint";
  posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
  posture.joint_names[5] = "r_gripper_l_finger_tip_joint";

  posture.points.resize(1);
  posture.points[0].positions.resize(6);
  posture.points[0].positions[0] = 1;
  posture.points[0].positions[1] = 1.0;
  posture.points[0].positions[2] = 0.477;
  posture.points[0].positions[3] = 0.477;
  posture.points[0].positions[4] = 0.477;
  posture.points[0].positions[5] = 0.477;
 }

void closedGripper(trajectory_msgs::JointTrajectory& posture){
  posture.joint_names.resize(6);
  posture.joint_names[0] = "r_gripper_joint";
  posture.joint_names[1] = "r_gripper_motor_screw_joint";
  posture.joint_names[2] = "r_gripper_l_finger_joint";
  posture.joint_names[3] = "r_gripper_r_finger_joint";
  posture.joint_names[4] = "r_gripper_r_finger_tip_joint";
  posture.joint_names[5] = "r_gripper_l_finger_tip_joint";

  posture.points.resize(1);
  posture.points[0].positions.resize(6);
  posture.points[0].positions[0] = 0;
  posture.points[0].positions[1] = 0;
  posture.points[0].positions[2] = 0.002;
  posture.points[0].positions[3] = 0.002;
  posture.points[0].positions[4] = 0.002;
  posture.points[0].positions[5] = 0.002;
}

我在这里要做的是尝试通过调用 OpenGripper 和 closedGripper 函数来打开和关闭机器人的抓手。这些函数在 opGripper 和 closGripper 函数的 cpp 文件中被调用。因此，我需要从 python 文件中调用 opGripper 和 closGripper 函数，以便从 C++ 文件中执行它们并执行任何必要的操作。我怎么做？

Answer 1

我要做的是将您的 C++ 代码包装在一个节点中，该节点公开打开和关闭抓手服务。

您可以从您的 python 代码或任何其他语言的任何其他 ROS 代码调用该服务。该行为不如调用原始 C++ 高效，但您已经从 python 调用它，因此延迟可以忽略不计。它的行为就像您直接调用该函数一样，因为您的代码将阻塞，直到服务返回。此外，您将能够保持这些接口(interface)语言独立，这使得事情更容易维护。

Writing a simple service and client