diff --git a/src/sorting_demo/launch/robot_sorting_demo_control.launch b/src/sorting_demo/launch/robot_sorting_demo_control.launch
index b854a6c..1f549f3 100644
--- a/src/sorting_demo/launch/robot_sorting_demo_control.launch
+++ b/src/sorting_demo/launch/robot_sorting_demo_control.launch
@@ -1,6 +1,6 @@
-
-
+
+
@@ -9,10 +9,12 @@
-
-
+
+
+
-
-
-
+
+
+
+
\ No newline at end of file
diff --git a/src/sorting_demo/launch/sawyer_moveit.launch b/src/sorting_demo/launch/sawyer_moveit.launch
new file mode 100644
index 0000000..50b80a5
--- /dev/null
+++ b/src/sorting_demo/launch/sawyer_moveit.launch
@@ -0,0 +1,52 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/sorting_demo/launch/sorting_demo.launch b/src/sorting_demo/launch/sorting_demo.launch
index 7bb5f6f..76ff808 100644
--- a/src/sorting_demo/launch/sorting_demo.launch
+++ b/src/sorting_demo/launch/sorting_demo.launch
@@ -1,9 +1,15 @@
-
-
-
+
-
-
+
+
+
+
+
+
+
+
+
+
diff --git a/src/sorting_demo/package.xml b/src/sorting_demo/package.xml
index 9c5e780..0bad465 100644
--- a/src/sorting_demo/package.xml
+++ b/src/sorting_demo/package.xml
@@ -63,10 +63,13 @@
gazebo_ros_control
gazebo_plugins
moveit
- rospy-message-converter
+ rospy_message_converter
gazebo_plugins
python-flask
python-catkin-tools
+ intera_interface
+ timed_roslaunch
+
rospy
rospack
@@ -77,6 +80,7 @@
gazebo_msgs
joint_state_controller
moveit
+ timed_roslaunch
position_controllers
velocity_controllers
@@ -85,9 +89,8 @@
ros_control
joint_trajectory_controller
gripper_action_controller
- rospy-message-converter
-
-
+ rospy_message_converter
+ intera_interface
diff --git a/src/sorting_demo/src/sorting_demo/environment_estimation.py b/src/sorting_demo/src/sorting_demo/environment_estimation.py
index 35caefb..3297e0e 100644
--- a/src/sorting_demo/src/sorting_demo/environment_estimation.py
+++ b/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))
@@ -466,4 +468,4 @@ class EnvironmentEstimation:
"""
:return array of (name, geometry_msgs.msg.Pose)
"""
- return self.trays
+ return self.trays
\ No newline at end of file
diff --git a/src/sorting_demo/src/sorting_demo/program.py b/src/sorting_demo/src/sorting_demo/program.py
index 47df9cd..38b1177 100755
--- a/src/sorting_demo/src/sorting_demo/program.py
+++ b/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()
diff --git a/src/sorting_demo/src/sorting_demo/task_planner.py b/src/sorting_demo/src/sorting_demo/task_planner.py
index 2a205a6..09c1777 100644
--- a/src/sorting_demo/src/sorting_demo/task_planner.py
+++ b/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)
diff --git a/src/sorting_demo/src/sorting_demo/trajectory_action_server.py b/src/sorting_demo/src/sorting_demo/trajectory_action_server.py
new file mode 100755
index 0000000..a2c6dd0
--- /dev/null
+++ b/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()