diff --git a/ork_tabletop_actionlib_server/CMakeLists.txt b/ork_tabletop_actionlib_server/CMakeLists.txt new file mode 100644 index 0000000..d626750 --- /dev/null +++ b/ork_tabletop_actionlib_server/CMakeLists.txt @@ -0,0 +1,174 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ork_tabletop_actionlib_server) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + actionlib + rospy + actionlib_msgs + object_recognition_msgs + visualization_msgs + sensor_msgs + geometry_msgs + tf + actionlib_msgs + shared_autonomy_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# DIRECTORY action +# FILES +# tabletop.action +# ) + +## Generate added messages and services with any dependencies listed here +#generate_messages( +# DEPENDENCIES +# actionlib_msgs object_recognition_msgs sensor_msgs geometry_msgs visualization_msgs actionlib shared_autonomy_msgs# Or other packages containing msgs +#) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES ork_tabletop_actionlib_server + CATKIN_DEPENDS actionlib_msgs object_recognition_msgs sensor_msgs geometry_msgs visualization_msgs actionlib shared_autonomy_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(ork_tabletop_actionlib_server +# src/${PROJECT_NAME}/ork_tabletop_actionlib_server.cpp +# ) + +## Declare a cpp executable +# add_executable(ork_tabletop_actionlib_server_node src/ork_tabletop_actionlib_server_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(ork_tabletop_actionlib_server_node ork_tabletop_actionlib_server_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(ork_tabletop_actionlib_server_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ork_tabletop_actionlib_server ork_tabletop_actionlib_server_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ork_tabletop_actionlib_server.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) +# install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/ork_tabletop_actionlib_server/msg +# DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_INSTALL_DIR}/ork_tabletop_actionlib_server) +# install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}/ork_tabletop_actionlib_server/action +# DESTINATION ${CMAKE_INSTALL_PREFIX}/${PYTHON_INSTALL_DIR}/ork_tabletop_actionlib_server) \ No newline at end of file diff --git a/ork_tabletop_actionlib_server/README.md b/ork_tabletop_actionlib_server/README.md new file mode 100644 index 0000000..a96911d --- /dev/null +++ b/ork_tabletop_actionlib_server/README.md @@ -0,0 +1,31 @@ +Steps to test this package: + +### Setup + +Install ORK: + + * mkdir catkin_hydro && cd catkin_hydro + * wstool init src https://raw.github.com/wg-perception/object_recognition_core/master/doc/source/ork.rosinstall + * cd src && wstool update -j8 + * cd .. && rosdep install --from-paths src -i -y + +Install shared_autonomy_perception: + + * `git clone git@github.com:SharedAutonomyToolkit/shared_autonomy_perception.git -b feature_ork_actionlib_server` + +Patch ORK: + + * source catkin_hydro/devel/setup.bash + * roscd object_recognition_tabletop + * patch -p1 -i \`rospack find ork_tabletop_actionlib_server\`/patch/clusters.patch + * cd catkin_hydro + * catkin_make + +### Execution: + + * robot_term1: `roslaunch /etc/ros/robot.launch` + * robot_term2: rosrun object_recognition_core detection -c \`rospack find object_recognition_tabletop\`/conf/detection.object.ros.ork + * robot_term3: `roslaunch shared_autonomy_launch openni_segmentation.launch` + * robot_term4: `rosrun ork_tabletop_actionlib_server ork_tabletop_actionlib_server.py` + * robot_term5: `rosrun ork_tabletop_actionlib_server test_client.py` + diff --git a/ork_tabletop_actionlib_server/package.xml b/ork_tabletop_actionlib_server/package.xml new file mode 100644 index 0000000..7c4a880 --- /dev/null +++ b/ork_tabletop_actionlib_server/package.xml @@ -0,0 +1,70 @@ + + + ork_tabletop_actionlib_server + 0.0.0 + The ork_tabletop_actionlib_server package + + + + + dejan + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + actionlib + actionlib_msgs + rospy + object_recognition_msgs + visualization_msgs + geometry_msgs + sensor_msgs + tf + shared_autonomy_msgs + actionlib + actionlib_msgs + rospy + object_recognition_msgs + visualization_msgs + geometry_msgs + sensor_msgs + tf + shared_autonomy_msgs + + + + + + + + + + \ No newline at end of file diff --git a/ork_tabletop_actionlib_server/patch/clusters.patch b/ork_tabletop_actionlib_server/patch/clusters.patch new file mode 100644 index 0000000..b14bb62 --- /dev/null +++ b/ork_tabletop_actionlib_server/patch/clusters.patch @@ -0,0 +1,185 @@ +diff --git a/conf/detection.object.ros.ork b/conf/detection.object.ros.ork +index 2c63d6f..22fb395 100644 +--- a/conf/detection.object.ros.ork ++++ b/conf/detection.object.ros.ork +@@ -5,11 +5,10 @@ source1: + # Example parameters to set (the default settings are using the ros topics starting with /camera/....) + # + parameters: +- #rgb_frame_id: camera_rgb_optical_frame +- rgb_image_topic: camera_rgb_image_rect_color +- rgb_camera_info: camera_rgb_camera_info +- depth_image_topic: camera_depth_registered_image_rect +- depth_camera_info: camera_depth_registered_camera_info ++ rgb_image_topic: '/head_mount_kinect/rgb/image_rect_color' ++ rgb_camera_info: '/head_mount_kinect/rgb/camera_info' ++ depth_image_topic: '/head_mount_kinect/depth_registered/image_raw' ++ depth_camera_info: '/head_mount_kinect/depth_registered/camera_info' + # + #crop_enabled: True + #x_min: -0.4 +@@ -39,9 +38,11 @@ pipeline1: + table_detector: + min_table_size: 4000 + plane_threshold: 0.01 ++ robot_frame: 'base_link' ++ sensor_frame: 'head_mount_kinect_rgb_optical_frame' + #clusterer: +- # table_z_filter_max: 0.35 +- # table_z_filter_min: 0.025 ++ # table_z_filter_max: 1.0 ++ # table_z_filter_min: 0.5 + + pipeline2: + type: TabletopObjectDetector +@@ -53,9 +54,9 @@ pipeline2: + tabletop_object_ids: 'REDUCED_MODEL_SET' + db: + type: 'ObjectDbSqlHousehold' +- host: 'wgs36' ++ host: 'beorn' + port: 5432 +- user: 'willow' +- password: 'willow' ++ user: 'bosch' ++ password: 'bosch' + name: 'household_objects' + module: 'object_recognition_tabletop' +diff --git a/conf/detection.table.ros.ork b/conf/detection.table.ros.ork +index 6551afc..0e33366 100644 +--- a/conf/detection.table.ros.ork ++++ b/conf/detection.table.ros.ork +@@ -4,12 +4,11 @@ source1: + # + # Example parameters to set (the default settings are using the ros topics starting with /camera/....) + # +- #parameters: +- #rgb_frame_id: '/kinect_head_rgb_optical_frame' +- #rgb_image_topic: '/kinect_head/rgb/image_rect_color' +- #rgb_camera_info: '/kinect_head/rgb/camera_info' +- #depth_image_topic: '/kinect_head/depth_registered/image_rect' +- #depth_camera_info: '/kinect_head/depth_registered/camera_info' ++ parameters: ++ rgb_image_topic: '/head_mount_kinect/rgb/image_rect_color' ++ rgb_camera_info: '/head_mount_kinect/rgb/camera_info' ++ depth_image_topic: '/head_mount_kinect/depth_registered/image_raw' ++ depth_camera_info: '/head_mount_kinect/depth_registered/camera_info' + # + #crop_enabled: True + #x_min: -0.4 +@@ -33,6 +32,8 @@ pipeline1: + table_detector: + min_table_size: 4000 + plane_threshold: 0.01 ++ robot_frame: 'base_link' ++ sensor_frame: 'head_mount_kinect_rgb_optical_frame' + #clusterer: + # table_z_filter_max: 0.35 + # table_z_filter_min: 0.025 +diff --git a/launch/table.launch b/launch/table.launch +index f8ae294..4d65856 100644 +--- a/launch/table.launch ++++ b/launch/table.launch +@@ -1,8 +1,10 @@ + +- +- +- +- +- ++ ++ ++ ++ ++ ++ ++ + + +diff --git a/src/table/TableDetector.cpp b/src/table/TableDetector.cpp +index c79096b..8ab7e9e 100644 +--- a/src/table/TableDetector.cpp ++++ b/src/table/TableDetector.cpp +@@ -58,7 +58,9 @@ namespace tabletop + "The distance used as a threshold when finding a plane", 0.02); + params.declare(&TableDetector::table_cluster_tolerance_, "table_cluster_tolerance", + "The distance used when clustering a plane", 0.2); +- params.declare(&TableDetector::up_frame_id_, "vertical_frame_id", "The vertical frame id", "/map"); ++ params.declare(&TableDetector::up_frame_id_, "vertical_frame_id", "The vertical frame id", "/base_link"); ++ params.declare(&TableDetector::robot_frame_, "robot_frame", "The robot frame", "base_link"); ++ params.declare(&TableDetector::sensor_frame_, "sensor_frame", "The sensor frame", "head_mount_kinect_rgb_optical_frame"); + } + + static void +@@ -79,7 +81,7 @@ namespace tabletop + ros::NodeHandle nh("~"); + nh.param("filter_planes", filter_planes_, true); + nh.param("min_table_height", min_table_height_, 0.5); +- nh.param("max_table_height", max_table_height_, 1.0); ++ nh.param("max_table_height", max_table_height_, 0.7); + nh.param("robot_frame", robot_frame_id_, std::string("/base_link")); + nh.param("sensor_frame", sensor_frame_id_, std::string("/head_mount_kinect_rgb_optical_frame")); + +@@ -96,7 +98,7 @@ namespace tabletop + + axis_ = tf::Vector3 (table_normal_x, table_normal_y, table_normal_z); + std::cout << __LINE__ << " :: " << min_table_height_ << " , " << max_table_height_ << " , " << min_angle_cos_ +- << " , " << robot_frame_id_ << " , " << sensor_frame_id_ << std::endl; ++ << " , " << *robot_frame_ << " , " << *sensor_frame_ << std::endl; + } + + /** Get the 2d keypoints and figure out their 3D position from the depth map +@@ -107,9 +109,10 @@ namespace tabletop + int + process(const tendrils& inputs, const tendrils& outputs) + { ++ std::cerr << "TableDetector: in process" << std::endl; + clouds_hull_->clear(); + table_coefficients_->clear(); +- if (!filter_planes_ || tf_->waitForTransform(robot_frame_id_, sensor_frame_id_, ros::Time(0), ros::Duration(0.5))) ++ if (!filter_planes_ || tf_->waitForTransform(*robot_frame_, *sensor_frame_, ros::Time(0), ros::Duration(0.5))) + { + if ((points3d_->rows != prev_image_rows_) || (points3d_->cols != prev_image_cols_)) + { +@@ -139,7 +142,7 @@ namespace tabletop + { + valid_planes = std::vector(plane_coefficients.size(), false); + tf::StampedTransform transform; +- tf_->lookupTransform(robot_frame_id_, sensor_frame_id_,ros::Time(0), transform); ++ tf_->lookupTransform(*robot_frame_, *sensor_frame_,ros::Time(0), transform); + tf::Matrix3x3 basis = transform.getBasis(); + tf::Vector3 origin = transform.getOrigin(); + +@@ -150,8 +153,12 @@ namespace tabletop + + tf::Vector3 normal_ = basis * normal; + double dist_ = normal_.dot (origin) - dist; +- if (normal_.dot(axis_) >= min_angle_cos_ && dist_ >= min_table_height_ && dist_ <= max_table_height_) ++ std::cerr << "dist: " << dist_ << " " << "min_angle_cos_: " << min_angle_cos_ << std::endl; ++ std::cerr << "min_table_height_: " << min_table_height_ << " " << "max_table_height_: " << max_table_height_ << std::endl; ++ // if (normal_.dot(axis_) >= min_angle_cos_ && dist_ >= min_table_height_ && dist_ <= max_table_height_) ++ if (dist_ >= min_table_height_ && dist_ <= max_table_height_) + { ++ std::cerr << "valid_plane_count: " << valid_plane_count << std::endl; + valid_planes [pIdx] = true; + ++valid_plane_count; + } +@@ -163,6 +170,7 @@ namespace tabletop + valid_plane_count = plane_coefficients.size(); + } + ++ std::cerr << "valid_plane_count: " << valid_plane_count << std::endl; + if (valid_plane_count > 0) + { + // Figure out the points of each plane +@@ -259,6 +267,10 @@ namespace tabletop + /** The frame id of the vertical direction */ + ecto::spore up_frame_id_; + ++ ecto::spore robot_frame_; ++ ++ ecto::spore sensor_frame_; ++ + ecto::spore table_cluster_tolerance_; + + /** Cache the size of the previous image */ diff --git a/ork_tabletop_actionlib_server/scripts/ork_tabletop_actionlib_server.py b/ork_tabletop_actionlib_server/scripts/ork_tabletop_actionlib_server.py new file mode 100755 index 0000000..c14f0d7 --- /dev/null +++ b/ork_tabletop_actionlib_server/scripts/ork_tabletop_actionlib_server.py @@ -0,0 +1,207 @@ +#! /usr/bin/env python + +import numpy as np +import threading +import sys + +import rospy +import actionlib + +from tf import TransformListener + +from object_recognition_msgs.msg import RecognizedObjectArray +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PoseStamped, PointStamped, Point, TransformStamped +from geometry_msgs.msg import PointStamped +from geometry_msgs.msg import Point +from geometry_msgs.msg import TransformStamped +from shared_autonomy_msgs.msg import TabletopAction, TabletopResult +from visualization_msgs.msg import MarkerArray + +# TODO: Fix the module organization of this package (this belongs in ../src/ork_tabletop_action_server/) +# TODO: there's some other pointcloud2 handling code in clear_table ... is there some canonical tool for this, so each module doesn't have its own library? +#from ork_tabletop_actionlib_server import pointclouds +import pointclouds + +class ORKTabletop(object): + """ Listens to the table and object messages from ORK. Provides ActionServer + that assembles table and object into same message. + NB - the table is an axis-aligned bounding box in the kinect's frame""" + def __init__(self, name): + + self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.callback) + self.pub = rospy.Publisher('/recognized_object_array_as_point_cloud', PointCloud2) + self.pose_pub = rospy.Publisher('/recognized_object_array_as_pose_stamped', PoseStamped) + + # We listen for ORK's MarkerArray of tables on this topic + self.table_topic = "/marker_tables" + + self.tl = TransformListener() + + # create messages that are used to publish feedback/result. + # accessed by multiple threads + self._result = TabletopResult() + self.result_lock = threading.Lock() + # used s.t. we don't return a _result message that hasn't been updated yet. + self.has_data = False + + self._action_name = name + self._as = actionlib.SimpleActionServer(self._action_name, TabletopAction, + execute_cb=self.execute_cb, auto_start=False) + self._as.start() + + # TODO: Is this really the best structure for handling the callbacks? + # Would it be possible to have separate callbacks for table and objects, each updating a most-recently-seen variable? + # Or maybe use the message_filters.TimeSynchronizer class if corresponding table/object data has identical timestamps? + def callback(self, data): + rospy.loginfo("Objects %d", data.objects.__len__()) + + table_corners = [] + + # obtain table_offset and table_pose + to = rospy.wait_for_message(self.table_topic, MarkerArray); + + # obtain Table corners ... + rospy.loginfo("Tables hull size %d", to.markers.__len__()) + if not to.markers: + rospy.loginfo("No tables detected") + return + else: + # NB - D says that ORK has this set to filter based on height. + # IIRC, it's 0.6-0.8m above whatever frame is set as the floor + point_array_size = 4 # for the 4 corners of the bounding box + for i in range (0, point_array_size): + p = Point() + p.x = to.markers[0].points[i].x + p.y = to.markers[0].points[i].y + p.z = to.markers[0].points[i].z + table_corners.append(p) + # this is a table pose at the edge close to the robot, in the center of x axis + table_pose = PoseStamped() + table_pose.header = to.markers[0].header + table_pose.pose = to.markers[0].pose + + # Determine table dimensions + rospy.loginfo('calculating table pose bounding box in frame: %s' % table_pose.header.frame_id) + min_x = sys.float_info.max + min_y = sys.float_info.max + max_x = -sys.float_info.max + max_y = -sys.float_info.max + + for i in range (table_corners.__len__()): + if table_corners[i].x > max_x: + max_x = table_corners[i].x + if table_corners[i].y > max_y: + max_y = table_corners[i].y + if table_corners[i].x < min_x: + min_x = table_corners[i].x + if table_corners[i].y < min_y: + min_y = table_corners[i].y + + table_dim = Point() + # TODO: if we don't (can't!) compute the height, should we at least give it non-zero depth? + # (would also require shifting the observed centroid down by table_dim.z/2) + table_dim.z = 0.0 + + table_dim.x = abs(max_x - min_x) + table_dim.y = abs(max_y - min_y) + print "Dimensions: ", table_dim.x, table_dim.y + + # Temporary frame used for transformations + table_link = 'table_link' + + # centroid of a table in table_link frame + centroid = PoseStamped() + centroid.header.frame_id = table_link + centroid.header.stamp = table_pose.header.stamp + centroid.pose.position.x = (max_x + min_x)/2. + centroid.pose.position.y = (max_y + min_y)/2. + centroid.pose.position.z = 0.0 + centroid.pose.orientation.x = 0.0 + centroid.pose.orientation.y = 0.0 + centroid.pose.orientation.z = 0.0 + centroid.pose.orientation.w = 1.0 + + # generate transform from table_pose to our newly-defined table_link + tt = TransformStamped() + tt.header = table_pose.header + tt.child_frame_id = table_link + tt.transform.translation = table_pose.pose.position + tt.transform.rotation = table_pose.pose.orientation + self.tl.setTransform(tt) + self.tl.waitForTransform(table_link, table_pose.header.frame_id, table_pose.header.stamp, rospy.Duration(3.0)) + if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): + centroid_table_pose = self.tl.transformPose(table_pose.header.frame_id, centroid) + self.pose_pub.publish(centroid_table_pose) + else: + rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) + return + + # transform each object into desired frame; add to list of clusters + cluster_list = [] + for i in range (data.objects.__len__()): + rospy.loginfo("Point clouds %s", data.objects[i].point_clouds.__len__()) + + pc = PointCloud2() + pc = data.objects[i].point_clouds[0] + arr = pointclouds.pointcloud2_to_array(pc, 1) + arr_xyz = pointclouds.get_xyz_points(arr) + + arr_xyz_trans = [] + for j in range (arr_xyz.__len__()): + ps = PointStamped(); + ps.header.frame_id = table_link + ps.header.stamp = table_pose.header.stamp + ps.point.x = arr_xyz[j][0] + ps.point.y = arr_xyz[j][1] + ps.point.z = arr_xyz[j][2] + if self.tl.canTransform(table_pose.header.frame_id, table_link, table_pose.header.stamp): + ps_in_kinect_frame = self.tl.transformPoint(table_pose.header.frame_id, ps) + else: + rospy.logwarn("No transform between %s and %s possible",table_pose.header.frame_id, table_link) + return + arr_xyz_trans.append([ps_in_kinect_frame.point.x, ps_in_kinect_frame.point.y, ps_in_kinect_frame.point.z]) + + pc_trans = PointCloud2() + pc_trans = pointclouds.xyz_array_to_pointcloud2(np.asarray([arr_xyz_trans]), + table_pose.header.stamp, table_pose.header.frame_id) + + self.pub.publish(pc_trans) + cluster_list.append(pc_trans) + rospy.loginfo("cluster size %d", cluster_list.__len__()) + + # finally - save all data in the object that'll be sent in response to actionserver requests + with self.result_lock: + self._result.objects = cluster_list + self._result.table_dims = table_dim + self._result.table_pose = centroid_table_pose + self.has_data = True + + def execute_cb(self, goal): + rospy.loginfo('Executing ORKTabletop action') + + # want to get the NEXT data coming in, rather than the current one. + with self.result_lock: + self.has_data = False + + rr = rospy.Rate(1.0) + while not rospy.is_shutdown() and not self._as.is_preempt_requested(): + with self.result_lock: + if self.has_data: + break + rr.sleep() + + if self._as.is_preempt_requested(): + rospy.loginfo('%s: Preempted' % self._action_name) + self._as.set_preempted() + elif rospy.is_shutdown(): + self._as.set_aborted() + else: + with self.result_lock: + rospy.loginfo('%s: Succeeded' % self._action_name) + self._as.set_succeeded(self._result) + +if __name__ == '__main__': + rospy.init_node('ork_tabletop') + ORKTabletop(rospy.get_name()) + rospy.spin() diff --git a/ork_tabletop_actionlib_server/scripts/pointclouds.py b/ork_tabletop_actionlib_server/scripts/pointclouds.py new file mode 100644 index 0000000..cf09139 --- /dev/null +++ b/ork_tabletop_actionlib_server/scripts/pointclouds.py @@ -0,0 +1,109 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Jon Binney +''' +Functions for working with PointCloud2. +''' + +__docformat__ = "restructuredtext en" + +#import roslib; roslib.load_manifest('pr2_python') +import numpy as np +from sensor_msgs.msg import PointCloud2, PointField +from tf import transformations +from geometry_msgs.msg import Transform, Vector3, Quaternion +import math + +def pointcloud2_to_array(cloud_msg, padding=0): + ''' + Converts a rospy PointCloud2 message to a numpy recordarray + + Assumes all fields 32 bit floats, and there is no padding. + ''' + print "datatype: ", cloud_msg.fields[0].datatype + dtype_list = [(f.name, np.float32) for f in cloud_msg.fields] + for i in range(padding): + dtype_list.append((str(i), np.float32)) + # print "dtype_list: ", dtype_list + # print "len: ", cloud_msg.data.__len__() + # print "divided len", int(math.floor(cloud_msg.data.__len__()/16.0)) + # print "height, width, point_step: ", cloud_msg.height, cloud_msg.width, cloud_msg.point_step + cloud_arr = np.fromstring(cloud_msg.data, dtype_list) + return np.reshape(cloud_arr, (cloud_msg.height, cloud_msg.width)) + +def get_xyz_points(cloud_array, remove_nans=True): + ''' + Pulls out x, y, and z columns from the cloud recordarray, and returns a 3xN matrix. + ''' + # remove crap points + if remove_nans: + mask = np.isfinite(cloud_array['x']) & np.isfinite(cloud_array['y']) & np.isfinite(cloud_array['z']) + cloud_array = cloud_array[mask] + + # pull out x, y, and z values + points = np.zeros(list(cloud_array.shape) + [3], dtype=np.float) + points[...,0] = cloud_array['x'] + points[...,1] = cloud_array['y'] + points[...,2] = cloud_array['z'] + + return points + +def pointcloud2_to_xyz_array(cloud_msg, remove_nans=True): + return get_xyz_points(pointcloud2_to_array(cloud_msg), remove_nans=remove_nans) + +def xyz_array_to_pointcloud2(points, stamp=None, frame_id=None): + ''' + Create a sensor_msgs.PointCloud2 from an array + of points. + ''' + msg = PointCloud2() + if stamp: + msg.header.stamp = stamp + if frame_id: + msg.header.frame_id = frame_id + if len(points.shape) == 3: + msg.height = points.shape[1] + msg.width = points.shape[0] + else: + msg.height = 1 + msg.width = len(points) + msg.fields = [ + PointField('x', 0, PointField.FLOAT32, 1), + PointField('y', 4, PointField.FLOAT32, 1), + PointField('z', 8, PointField.FLOAT32, 1)] + msg.is_bigendian = False + msg.point_step = 12 + msg.row_step = 12*points.shape[0] + msg.is_dense = int(np.isfinite(points).all()) + msg.data = np.asarray(points, np.float32).tostring() + return msg diff --git a/ork_tabletop_actionlib_server/scripts/test_client.py b/ork_tabletop_actionlib_server/scripts/test_client.py new file mode 100755 index 0000000..efed72b --- /dev/null +++ b/ork_tabletop_actionlib_server/scripts/test_client.py @@ -0,0 +1,44 @@ +#! /usr/bin/env python + +import rospy + +# Brings in the SimpleActionClient +import actionlib + +# Brings in the messages used by the fibonacci action, including the +# goal message and the result message. +import shared_autonomy_msgs.msg + +def tabletop_client(): + # Creates the SimpleActionClient, passing the type of the action + client = actionlib.SimpleActionClient('ork_tabletop', shared_autonomy_msgs.msg.TabletopAction) + + # Waits until the action server has started up and started + # listening for goals. + client.wait_for_server() + + # Creates a goal to send to the action server. + goal = shared_autonomy_msgs.msg.TabletopGoal() + + # Sends the goal to the action server. + client.send_goal(goal) + + # Waits for the server to finish performing the action. + client.wait_for_result() + + # Prints out the result of executing the action + return client.get_result() + +if __name__ == '__main__': + try: + # Initializes a rospy node so that the SimpleActionClient can + # publish and subscribe over ROS. + rospy.init_node('tabletop_client_py') + result = tabletop_client() + print "Table pose: " + print result.table_pose + print "Table dims: " + print result.table_dims + print "Num Objects: ", result.objects.__len__() + except rospy.ROSInterruptException: + print "program interrupted before completion" diff --git a/ork_tabletop_actionlib_server/setup.py b/ork_tabletop_actionlib_server/setup.py new file mode 100644 index 0000000..9818644 --- /dev/null +++ b/ork_tabletop_actionlib_server/setup.py @@ -0,0 +1,11 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['ork_tabletop_actionlib_server'], + package_dir={'': 'src'}, + requires=['actionlib', 'actionlib_msgs', 'geometry_msgs', 'rospy', 'tf', 'object_recognition_msgs', 'sensor_msgs', 'visualization_msgs', 'shared_autonomy_msgs'], + scripts=['scripts/ork_tabletop_actionlib_server.py'] +) + +setup(**d) diff --git a/shared_autonomy_msgs/CMakeLists.txt b/shared_autonomy_msgs/CMakeLists.txt index 13ad980..680f298 100644 --- a/shared_autonomy_msgs/CMakeLists.txt +++ b/shared_autonomy_msgs/CMakeLists.txt @@ -4,7 +4,7 @@ project(shared_autonomy_msgs) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS message_generation sensor_msgs actionlib_msgs std_msgs) +find_package(catkin REQUIRED COMPONENTS message_generation sensor_msgs actionlib_msgs std_msgs geometry_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -21,7 +21,7 @@ find_package(catkin REQUIRED COMPONENTS message_generation sensor_msgs actionlib add_action_files( DIRECTORY action - FILES Segment.action BoundingBox.action EditPixel.action + FILES Segment.action BoundingBox.action EditPixel.action Tabletop.action ) ## Generate messages in the 'msg' folder @@ -38,7 +38,7 @@ add_service_files( ## Generate added messages and services with any dependencies listed here generate_messages( - DEPENDENCIES sensor_msgs std_msgs actionlib_msgs std_msgs + DEPENDENCIES sensor_msgs std_msgs actionlib_msgs geometry_msgs ) ################################### @@ -53,7 +53,7 @@ generate_messages( catkin_package( # INCLUDE_DIRS include # LIBRARIES shared_autonomy_msgs - CATKIN_DEPENDS sensor_msgs actionlib_msgs std_msgs + CATKIN_DEPENDS sensor_msgs actionlib_msgs std_msgs geometry_msgs # DEPENDS system_lib ) diff --git a/shared_autonomy_msgs/action/Tabletop.action b/shared_autonomy_msgs/action/Tabletop.action new file mode 100644 index 0000000..e406020 --- /dev/null +++ b/shared_autonomy_msgs/action/Tabletop.action @@ -0,0 +1,8 @@ +#goal definition +--- +#result definition +geometry_msgs/PoseStamped table_pose +geometry_msgs/Point table_dims +sensor_msgs/PointCloud2[] objects +--- +#feedback \ No newline at end of file diff --git a/shared_autonomy_msgs/package.xml b/shared_autonomy_msgs/package.xml index 909f536..15db9ab 100644 --- a/shared_autonomy_msgs/package.xml +++ b/shared_autonomy_msgs/package.xml @@ -46,12 +46,14 @@ message_generation sensor_msgs std_msgs - + geometry_msgs + actionlib_msgs geometry_msgs message_runtime sensor_msgs std_msgs + geometry_msgs