changes on sorting demo

src/sorting_demo/launch/robot_sorting_demo_control.launch

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="electric_gripper" default="false"/>
+  <arg name="visualization" default="true"/>
   
   <!-- Start the Sawyer pick and place demo -->
   <node pkg="sorting_demo" type="program.py" name="sorting_demo" >
@@ -9,10 +9,12 @@
   <node pkg="topic_tools" type="relay" args="/robot/joint_states /joint_states" name="statedup"/>
   <node pkg="sawyer_ik_5d" type="sawyer_ik_5d_node" name="sawyer_ik_5d_node"  output="screen"/>
 
-  <node pkg="intera_interface" type="joint_trajectory_action_server.py" name="trajectory_action_server"/>
+    <!---->
+    <node pkg="sorting_demo" type="trajectory_action_server.py" name="trajectory_action_server"/>
     <node pkg="sorting_demo" type="gripper_action_server.py" name="gripper_action_server"/>
 
-  <include file="$(find sawyer_moveit_config)/launch/sawyer_moveit.launch">
+    <include file="$(find sorting_demo)/launch/sawyer_moveit.launch">
-        <arg name="electric_gripper" value="$(arg electric_gripper)"/>
+        <arg name="electric_gripper" value="true"/>
+        <arg name="visualization" value="$(arg visualization)"/>
     </include>
 </launch>

src/sorting_demo/launch/sawyer_moveit.launch

@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="config" default="true"/>
+  <arg name="rviz_config" default="$(find sawyer_moveit_config)/launch/moveit.rviz" />
+  <arg name="visualization" default="true"/>
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="false"/>
+  <!-- Add controller box collision shape to check for link collisions if set to true-->
+  <arg name="controller_box" default="true"/>
+  <!-- Left and electric gripper arg. Set to true to check for collisions for their links -->
+  <arg name="electric_gripper" default="false"/>
+  <!-- Set the kinematic tip for the right_arm move_group -->
+  <arg name="tip_name"     if="$(arg electric_gripper)" default="right_gripper_tip"/>
+  <arg name="tip_name" unless="$(arg electric_gripper)" default="right_hand"/>
+  <!-- Add planning context launch file -->
+  <include file="$(find sawyer_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+    <arg name="electric_gripper" value="$(arg electric_gripper)"/>
+    <arg name="tip_name" value="$(arg tip_name)"/>
+    <arg name="controller_box" value="$(arg controller_box)"/>
+  </include>
+
+  <arg name="kinect" default="false" />
+  <arg name="xtion" default="false" />
+  <arg name="camera_link_pose" default="0.15 0.075 0.5 0.0 0.7854 0.0"/>
+  <include file="$(find sawyer_moveit_config)/launch/move_group.launch">
+    <arg name="kinect" value="$(arg kinect)" />
+    <arg name="xtion" value="$(arg xtion)" />
+    <arg name="camera_link_pose" default="$(arg camera_link_pose)"/>
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="pipeline" value="ompl" />
+  </include>
+
+  <include file="$(find sawyer_moveit_config)/launch/moveit_rviz.launch" if="$(arg visualization)">
+    <arg name="config" value="$(arg config)" />
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="rviz_config" value="$(arg rviz_config)" />
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find sawyer_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"/>
+
+</launch>

src/sorting_demo/launch/sorting_demo.launch

@@ -1,9 +1,15 @@
 <launch>
+<arg name="visualization" default="true"/>
+
   <include file="$(find sawyer_gazebo)/launch/sawyer_world.launch">
     <arg name="electric_gripper" value="true"/>    
+    <arg name="gui" value="$(arg visualization)"/>
   </include>
   
-  <include file="$(find sorting_demo)/launch/robot_sorting_demo_control.launch">
+  <include file="$(find timed_roslaunch)/launch/timed_roslaunch.launch">
-    <arg name="electric_gripper" value="true"/>
+    <arg name="time" value="5" />
+    <arg name="pkg" value="sorting_demo" />
+    <arg name="file" value="robot_sorting_demo_control.launch" />
+    <arg name="value" value="visualization:=$(arg visualization)" />
   </include>
 </launch>

src/sorting_demo/package.xml

@@ -63,10 +63,13 @@
   <build_depend>gazebo_ros_control</build_depend>
   <build_depend>gazebo_plugins</build_depend>
   <build_depend>moveit</build_depend>
-  <build_depend>rospy-message-converter</build_depend>
+  <build_depend>rospy_message_converter</build_depend>
   <build_depend>gazebo_plugins</build_depend>
   <build_depend>python-flask</build_depend>
   <build_depend>python-catkin-tools</build_depend>
+  <build_depend>intera_interface</build_depend>
+  <build_depend>timed_roslaunch</build_depend>
+
 
   <exec_depend>rospy</exec_depend>
   <exec_depend>rospack</exec_depend>
@@ -77,6 +80,7 @@
   <exec_depend>gazebo_msgs</exec_depend>
   <exec_depend>joint_state_controller</exec_depend>
   <exec_depend>moveit</exec_depend>
+  <exec_depend>timed_roslaunch</exec_depend>
 
   <exec_depend>position_controllers</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
@@ -85,9 +89,8 @@
   <exec_depend>ros_control</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>gripper_action_controller</exec_depend>
-  <exec_depend>rospy-message-converter</exec_depend>
+  <exec_depend>rospy_message_converter</exec_depend>
-
+  <exec_depend>intera_interface</exec_depend>
-
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/sorting_demo/src/sorting_demo/environment_estimation.py

@@ -22,7 +22,6 @@ import demo_constants
 from threading import RLock
 import cv2
 
-
 class EnvironmentEstimation:
     def __init__(self):
         """
@@ -174,11 +173,13 @@ class EnvironmentEstimation:
             # cv2.imshow("cube detection", cv_image)
             # cv2.waitKey(0)
 
+            #position = closest_block.gazebo_pose.position
+            #orientation = closest_block.gazebo_pose.orientation
 
             return Pose(position=Point(x=projected[0], y=projected[1], z=projected[1]),
                         orientation=Quaternion(x=poseq[0], y=poseq[1], z=poseq[2], w=poseq[3])), graspA or graspB
         except Exception as ex:
-            rospy.logwarn("erroneous cube detection")
+            rospy.logwarn("erroneous cube detection:  " + str(ex))
             # cv2.imshow("erroneus cube detection", cv_image)
             # cv2.waitKey(0)
             return None, False
@@ -438,6 +439,7 @@ class EnvironmentEstimation:
         :param id:
         :return:
         """
+
         color = color.replace("Gazebo/", "")
         rospy.logwarn("by color: " + str(color))
         rospy.logwarn("by color: " + str(self.trays))

src/sorting_demo/src/sorting_demo/program.py

@@ -1,14 +1,20 @@
 #!/usr/bin/python
+import sys
+import os
+
+# Assuming that pdvsd is located in the working folder
+sys.path.append(os.getcwd())
+
 import functools
 import sys
 import rospy
 
-from task_planner import TaskPlanner
 import demo_constants
 import gazebo_models
-
+from task_planner import TaskPlanner
 
 def main():
+
     rospy.init_node("sorting_demo")
     rospy.logwarn("Hello world")
 
@@ -30,7 +36,7 @@ def main():
 
     task_facade = task_planner.get_task_facade()
 
-    task_facade.start()
+    #task_facade.start()
 
     task_facade.run_rest_server()
 

src/sorting_demo/src/sorting_demo/task_planner.py

@@ -408,6 +408,7 @@ class TaskPlanner:
         :return:
         """
         # rotate in y
+
         reverseTransformY = utils.mathutils.rot_y(-1.3 * numpy.pi / 2.0)
         # reverseTransformY = utils.mathutils.rot_x(numpy.pi / 2.0)
         reverseTransformZ = numpy.eye(4)
@@ -859,7 +860,6 @@ class TaskPlanner:
         """
 
         try:
-
             self.moveit_tabletop_pick(target_block).result()
 
             rospy.sleep(0.1)

src/sorting_demo/src/sorting_demo/trajectory_action_server.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+# Copyright (c) 2013-2018, Rethink Robotics Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Intera SDK Joint Trajectory Controller
+    Unlike other robots running ROS, this is not a Motor Controller plugin,
+    but a regular node using the SDK interface.
+"""
+import argparse
+import importlib
+
+import rospy
+from dynamic_reconfigure.server import Server
+import intera_interface
+
+from intera_joint_trajectory_action.joint_trajectory_action import (
+    JointTrajectoryActionServer,
+)
+from trajectory_msgs.msg import (
+    JointTrajectoryPoint,
+)
+
+def start_server(limb, rate, mode, valid_limbs):
+    rospy.loginfo("Initializing node... ")
+    rospy.init_node("sdk_{0}_joint_trajectory_action_server{1}".format(
+                        mode, "" if limb == 'all_limbs' else "_" + limb,))
+
+    rospy.wait_for_service("/ExternalTools/right/PositionKinematicsNode/IKService")
+
+    rospy.loginfo("Initializing joint trajectory action server...")
+    robot_name = intera_interface.RobotParams().get_robot_name().lower().capitalize()
+    config_module = "intera_interface.cfg"
+    if mode == 'velocity':
+        config_name = ''.join([robot_name,"VelocityJointTrajectoryActionServerConfig"])
+    elif mode == 'position':
+        config_name = ''.join([robot_name,"PositionJointTrajectoryActionServerConfig"])
+    else:
+        config_name = ''.join([robot_name,"PositionFFJointTrajectoryActionServerConfig"])
+    cfg = importlib.import_module('.'.join([config_module,config_name]))
+    dyn_cfg_srv = Server(cfg, lambda config, level: config)
+    jtas = []
+    if limb == 'all_limbs':
+        for current_limb in valid_limbs:
+            jtas.append(JointTrajectoryActionServer(current_limb, dyn_cfg_srv,
+                                                    rate, mode))
+    else:
+        jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
+
+
+    def cleanup():
+        for j in jtas:
+            j.clean_shutdown()
+
+    rospy.on_shutdown(cleanup)
+    rospy.loginfo("Joint Trajectory Action Server Running. Ctrl-c to quit")
+    rospy.spin()
+
+
+def main():
+    rp = intera_interface.RobotParams()
+    valid_limbs = rp.get_limb_names()
+    if not valid_limbs:
+        rp.log_message(("Cannot detect any limb parameters on this robot. "
+                        "Exiting."), "ERROR")
+        return
+    # Add an option for starting a server for all valid limbs
+    all_limbs = valid_limbs
+    all_limbs.append("all_limbs")
+    arg_fmt = argparse.ArgumentDefaultsHelpFormatter
+    parser = argparse.ArgumentParser(formatter_class=arg_fmt)
+    parser.add_argument(
+        "-l", "--limb", dest="limb", default=valid_limbs[0],
+        choices=all_limbs,
+        help="joint trajectory action server limb"
+    )
+    parser.add_argument(
+        "-r", "--rate", dest="rate", default=100.0,
+        type=float, help="trajectory control rate (Hz)"
+    )
+    parser.add_argument(
+        "-m", "--mode", default='position_w_id',
+        choices=['position_w_id', 'position', 'velocity'],
+        help="control mode for trajectory execution"
+    )
+    args = parser.parse_args(rospy.myargv()[1:])
+
+    start_server(args.limb, args.rate, args.mode, valid_limbs)
+
+
+if __name__ == "__main__":
+    main()