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

Question

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

下面的代码在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函数来打开和关闭机器人的抓取器。这些函数在cpp文件中从opGripper和closGripper函数调用。因此，我需要从python文件调用opGripper和closGripper函数，以便从C++文件执行它们，并执行任何必要的操作。我该怎么做？

Answer 1

您可以使用任何语言从python代码或任何其他ROS代码调用服务。这种行为没有调用原始C++那么高效，但是您已经在python中调用它了，所以延迟可以忽略不计。它的行为就像您直接调用函数一样，因为您的代码将阻塞，直到服务返回。此外，您还可以保持这些接口与语言无关，这使得维护工作变得更容易。

Writing a simple service and client

Answer 2

我不知道这个问题是否已经解决了，但只是以防万一。

您可以使用Boost python库来实现您想要做的事情。您可以构建将python库链接到它的onGripper.so。然后，您可以在python代码的顶部导入onGripper。

以下是您放入C++ .hpp文件中的技巧：

1. #include<boost/python.hpp>
2. BOOST_PYTHON_MODULE(gpio)

  {  // the module is called gpio

       using namespace boost::python;     // require this namespace

       using namespace exploringRPi;      // bring in custom namespace

       class_<GPIO, boost::noncopyable>("GPIO", init<int>())

       .def("getNumber", &GPIO::getNumber)

       .def("setDirection", &GPIO::setDirection)

       .def("setValue", &GPIO::setValue)

       .def("getValue", &GPIO::getValue);
 }

请注意，gpio是类名；我创建了一个C++类来访问我的Raspberry pi上的GPIO。"getNumber"等是这个类的公共方法。