How to call a C++ function from a python (py) file?

Question

I am aware of the boost python library but what I need is to call a C++ function which is in a cpp file from a python file (py). Here is a simple python code that moves the arm of the PR2 robot.

The following code is in a py file

#!/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

Here is my c++ code in cpp file

#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;
}

What I am trying to do here is I am trying to open and close the gripper of the robot by calling OpenGripper and closedGripper functions. These functions are being called within the cpp file from opGripper and closGripper functions. So, I need to call the opGripper and closGripper functions from the python file so that they are executed from the C++ file and do whatever necessary. How do I do that?

Answer 1

I do not know if this question is already solved or not but just in case.

You can use Boost python library to implement what you want to do. You can build onGripper.so linking python library to it. Then you can import onGripper at the top of the python code.

Trick is the following that you put in the C++ .hpp file:

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);
 }

Note that gpio is the class name; I created a C++ class to access GPIO on my Raspberry pi. And "getNumber" and others are the public method of this class.

Answer 2

What I would do is wrap your C++ code in a node that exposes open and close gripper services.

You can call the service from your python code or any other ROS code in any language. The behavior isn't as efficient as calling raw C++ but you are already calling it from python so the latency will be negligible. It will behave as if you are calling the function directly, because your code will block until the service returns. Also you will be able to keep these interfaces language independent, which makes things easier to maintain.

Writing a simple service and client