Python interface

README.md

@@ -23,8 +23,8 @@
 
 ```shell
 
-# Install Ensenso SDK from!
-# https://www.ensenso.com/support/sdk-download
+# Install Ensenso SDK (from https://www.ensenso.com/support/sdk-download) by:
+wget -q https://download.ensenso.com/s/ensensosdk/download?files=ensenso-sdk-2.2.65-x64.deb -O /tmp/ensenso.deb && dpkg -i /tmp/ensenso.deb
 
 # Create a new ROS workspace
 mkdir -p ~/snp_demo_ws/src && cd ~/snp_demo_ws/src
@@ -66,9 +66,23 @@ roslaunch snp_prbt_bringup application_bringup.launch sim_robot:=false
 
 ```
 
-## Systems settings
+## Scan and Plan: Remove local scan_parameter cache
+Make sure to clear possible local `godel_robot_scan_parameters` since they would overwrite the ones of this repo (`snp_prbt_bringup/config/robot_scan.yaml`).
+
+```shell
+rm -f ~/.ros/godel_robot_scan_parameters.msg
+```
+
+## Systems settings for real devices
+
+### Camera: uEye Driver
+```
+wget -q https://download.ensenso.com/s/idsdrivers/download?files=uEye_4.90.0_Linux_64.tgz -O /tmp/ueye.tgz
+tar -xvzf /tmp/ueye.tgz  -C /tmp
+sudo /tmp/ueyesdk-setup-4.90-eth-amd64.gz.run
+```
 
-### CAN interface
+### Robot: CAN interface
 Add in `/etc/network/interfaces` the following config for the [socketcan_interface]( http://wiki.ros.org/socketcan_interface).
 
 ```
@@ -79,15 +93,4 @@ iface can0 can static
 ```
 
 
-### Remove local scan_parameter cache
-Make sure to clear possible local `godel_robot_scan_parameters` since they would overwrite the ones of this repo (`snp_prbt_bringup/config/robot_scan.yaml`).
-
-```shell
-rm -f ~/.ros/godel_robot_scan_parameters.msg
-```
-
-### Fix QT bug for Robot Planing Panel (might not be required anymore)
-```shell
-echo "export QT_NO_FT_CACHE=1" >> ~/.bashrc && source ~/.bashrc
-```
 

snp_prbt_bringup/launch/application_bringup.launch

@@ -2,15 +2,19 @@
 <launch>
 
   <arg name="sim_robot" default="true"/>
+  <arg name="gui" default="true"/>
+
+  <param name="sim_robot" type="string" value="$(arg sim_robot)"/>
 
   <include file="$(find snp_prbt_bringup)/launch/hardware_bringup.launch">
     <arg name="sim_robot" value="$(arg sim_robot)"/>
+    <arg name="gui" value="$(arg gui)"/>
   </include>
 
   <include file="$(find snp_prbt_bringup)/launch/prbt_godel.launch">
     <arg name="sim_robot" value="$(arg sim_robot)"/>
   </include>
 
-  <include file="$(find snp_prbt_bringup)/launch/rviz.launch"/>
+  <include  if="$(arg gui)" file="$(find snp_prbt_bringup)/launch/rviz.launch"/>
 
 </launch>

snp_prbt_bringup/launch/hardware_bringup.launch

@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 <launch>
 
+  <arg name="gui" default="true"/>
   <arg name="sim_robot" default="false"/>
   <arg name="pipeline" default="command_planner"/>
   <arg name="robot_name" default="prbt"/>
@@ -12,15 +13,17 @@
 
   <!-- Robot -->
   <group if="$(arg sim_robot)">
-    <include file="$(find snp_prbt_bringup)/launch/prbt_gazebo.launch"/>
+    <include file="$(find snp_prbt_bringup)/launch/prbt_gazebo.launch">
+      <arg name="gui" value="$(arg gui)"/>
+    </include>
   </group>
 
   <group unless="$(arg sim_robot)">
     <include file="$(find snp_prbt_bringup)/launch/prbt_real.launch">
       <arg name="robot_ip" value="169.254.60.1"/>
     </include>
   </group>
-  
+
   <rosparam command="load" file="$(find prbt_moveit_config)/config/joint_names.yaml"/>
 
   <include file="$(find snp_prbt_moveit_config)/launch/move_group.launch">
@@ -33,9 +36,9 @@
   <node name="snp_prbt_support" pkg="snp_prbt_support" type="spindle_sim" if="$(arg sim_robot)"/>
 
   <!-- camera -->
-  <include file="$(find snp_prbt_support)/launch/setup_camera.launch" unless="$(arg sim_robot)">
+  <!-- <include file="$(find snp_prbt_support)/launch/setup_camera.launch" unless="$(arg sim_robot)">
     <arg name="roi_lower" value="{'x': -0.16, 'y': -0.4, 'z': 0.0}"/>
     <arg name="roi_upper" value="{'x':  0.16, 'y':  0.4, 'z': 0.6}"/>
-  </include>
+  </include> -->
 
 </launch>

snp_prbt_bringup/launch/prbt_gazebo.launch

@@ -2,15 +2,14 @@
 <launch>
 
   <arg name="controller_group" default="manipulator"/>
+  <arg name="gui" default="true"/>
   <arg name="robot_name" default="prbt" />
   <arg name="paused" default="true"/>
-  <arg name="gui" default="false"/>
-
-  <param name="use_gui" value="$(arg gui)"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(find snp_prbt_description)/worlds/prbt_robot.world"/>
     <arg name="paused" value="$(arg paused)"/>
+    <arg name="gui" value="$(arg gui)"/>
   </include>
 
   <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model $(arg robot_name)

snp_py/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(snp_py)
+
+find_package(catkin REQUIRED)
+catkin_package()
+catkin_python_setup()
+
+catkin_install_python(PROGRAMS scripts/snp_auto_run
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

snp_py/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>snp_py</name>
+  <version>0.0.0</version>
+  <description>Scan and plan Python interface</description>
+  <maintainer email="[email protected]">Jonathan Hechtbauer</maintainer>
+  <author email="[email protected]">Jonathan Hechtbauer</author>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>godel_msgs</exec_depend>
+  <exec_depend>moveit_commander</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <!-- <exec_depend>snp_prbt_bringup</exec_depend>  Removed since bringup_pkg as arg
+  <exec_depend>snp_prbt_support</exec_depend> -->
+  <exec_depend>std_srvs</exec_depend>
+
+</package>

snp_py/scripts/snp_auto_run

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+import rospy
+from moveit_commander import MoveItCommanderException
+from snp_py.snp_interface import SNPCommander
+
+if __name__ == "__main__":
+    rospy.init_node("snp_test")
+    move_group = rospy.get_param("/godel_process_planning/blend_group")
+    SNP = SNPCommander(move_group, planner="PTP")
+    try:
+        SNP.move_home()
+        SNP.surface_detection()
+        SNP.select_surface()
+        SNP.plan()
+        SNP.get_available_motion_plan()
+        SNP.select_motion_plan(simulate=True)
+        SNP.select_motion_plan(simulate=False)
+
+    except (MoveItCommanderException, rospy.ROSException, rospy.ServiceException) as e:
+        rospy.logerr("SNP failed due to %s" % e)

snp_py/setup.py

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(packages=["snp_py"], package_dir={"": "src"})
+setup(**d)

snp_py/src/snp_py/snp_helper.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+import sys
+import rospy
+import time
+import moveit_commander
+import actionlib
+from moveit_msgs.msg import MoveGroupAction
+from std_srvs.srv import Empty, EmptyRequest
+
+
+def unpause_gazebo(delay):
+    # Give Gazebo enough time. Sim_time not running yet
+    time.sleep(delay)
+
+    # Pause is required to span model with defined joint_values
+    unpause = init_srv("/gazebo/unpause_physics", Empty)
+    unpause(EmptyRequest())
+
+
+def init_move_group(group_name):
+    # This is to make sure that MoveIt! has been started up.
+    init_action("/move_group", MoveGroupAction, timeout=60)
+
+    moveit_commander.roscpp_initialize(sys.argv)
+    robot = moveit_commander.RobotCommander()
+    return moveit_commander.MoveGroupCommander(group_name)
+
+
+def init_srv(srv_ns, srv_type):
+    rospy.loginfo("Wait for service '%s' ..." % srv_ns)
+    rospy.wait_for_service(srv_ns)
+    return rospy.ServiceProxy(srv_ns, srv_type)
+
+
+def init_action(act_ns, act_type, timeout=30):
+    client = actionlib.SimpleActionClient(act_ns, act_type)
+    rospy.loginfo("Wait for action '%s' ..." % act_ns)
+    server_up = client.wait_for_server(timeout=rospy.Duration(timeout))
+    if not server_up:
+        raise rospy.ROSInterruptException("Action timed out.")
+    return client
+
+
+def call_action(client, request, timeout):
+    client.send_goal(request)
+    action_returned = client.wait_for_result(timeout=rospy.Duration(timeout))
+    if not action_returned:
+        raise rospy.ROSInterruptException("Action timed out.")
+    return client.get_result()

snp_py/src/snp_py/snp_interface.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+
+import rospy
+import godel_msgs.srv as _s
+import godel_msgs.msg as _a
+from snp_py.snp_helper import (
+    init_move_group,
+    init_srv,
+    init_action,
+    call_action,
+    unpause_gazebo,
+)
+
+
+class SNPCommander:
+    def __init__(self, move_group, planner, delay_unpause_gazebo=0):
+        if not rospy.get_param("/sim_robot"):
+            raise rospy.ROSException("SNPCommander is only allowed for simulation.")
+
+        # Pause is required to span model with defined joint_values
+        unpause_gazebo(delay_unpause_gazebo)
+
+        self._group = init_move_group(move_group)
+        self._group.set_planner_id(planner)
+
+        self._surfaceDetection = init_srv("surface_detection", _s.SurfaceDetection)
+        self._selectSurface = init_srv("select_surface", _s.SelectSurface)
+        self._getAvailableMotionPlan = init_srv(
+            "get_available_motion_plans", _s.GetAvailableMotionPlans
+        )
+        self._plan = init_action("process_planning_as", _a.ProcessPlanningAction)
+        self._selectMotionPlan = init_action(
+            "select_motion_plan_as", _a.SelectMotionPlanAction
+        )
+        self._plans = []
+
+    def move_home(self):
+        self._group.set_named_target("home")
+        return self._group.go(wait=True) is not None
+
+    def surface_detection(self):
+        return self._surfaceDetection(
+            _s.SurfaceDetectionRequest(
+                action=_s.SurfaceDetectionRequest.SCAN_AND_FIND_ONLY,
+                use_default_parameters=True,
+            )
+        ).surfaces_found
+
+    def select_surface(self):
+        return self._selectSurface(
+            _s.SelectSurfaceRequest(action=_s.SelectSurfaceRequest.SELECT_ALL)
+        ).succeeded
+
+    def plan(self, timeout=120):
+        return call_action(
+            self._plan,
+            _a.ProcessPlanningGoal(
+                action=_a.ProcessPlanningGoal.GENERATE_MOTION_PLAN_AND_PREVIEW,
+                use_default_parameters=True,
+            ),
+            timeout,
+        ).succeeded
+
+    def get_available_motion_plan(self):
+        self._plans = self._getAvailableMotionPlan(
+            _s.GetAvailableMotionPlansRequest()
+        ).names
+        return self._plans != []
+
+    def select_motion_plan(self, simulate, timeout=120):
+        for p in self._plans:
+            result = call_action(
+                self._selectMotionPlan,
+                _a.SelectMotionPlanGoal(
+                    simulate=simulate, wait_for_execution=True, name=p
+                ),
+                timeout,
+            )
+            if result.code != _a.SelectMotionPlanResult.SUCCESS:
+                return False
+        return True
+