blue container bundle

art_arcode_detector/bundles/blue_container.xml

@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<multimarker markers="2">
+    <marker index="5001" status="2">
+         <corner x="-1.9" y="-1.9" z="0" />
+        <corner x="1.9" y="-1.9" z="0" />
+        <corner x="1.9" y="1.9" z="0" />
+        <corner x="-1.9" y="1.9" z="0" />
+    </marker>
+
+    <marker index="122" status="2">
+        <corner x="-6.9" y="4.1" z="0" />
+        <corner x="-3.1" y="4.1" z="0" />
+        <corner x="-3.1" y="7.9" z="0" />
+        <corner x="-6.9" y="7.9" z="0" />
+    </marker>
+
+
+
+
+</multimarker>

art_presence_detector/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(art_presence_detector)
+
+find_package(catkin REQUIRED COMPONENTS
+  pcl_ros
+  roscpp
+  art_msgs
+  std_msgs
+  sensor_msgs
+  pcl_conversions
+)
+
+
+
+add_definitions(
+  ${PCL_DEFINITIONS}
+)
+
+catkin_package(
+  CATKIN_DEPENDS art_msgs std_msgs
+)
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_executable(${PROJECT_NAME}_node src/node.cpp)
+
+target_link_libraries(${PROJECT_NAME}_node
+   ${catkin_LIBRARIES}
+)

art_presence_detector/launch/art_presence_detector.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+
+    <arg name="debug" default="true"/>
+
+    <node pkg="art_presence_detector" name="art_presence_detector" type="art_presence_detector_node" output="screen">
+
+    </node>
+    <node if="$(arg debug)" pkg="tf" type="static_transform_publisher" name="marker_link" args="0 0 1.2 0 3.14 0 /marker /kinect2_link 100"/>
+    <node if="$(arg debug)" pkg="tf" type="static_transform_publisher" name="world_link" args="0 0 0 0 0 0 /marker /world 100"/>
+
+</launch>

art_presence_detector/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>art_presence_detector</name>
+  <version>0.0.0</version>
+  <description>The art_presence_detector package</description>
+
+  <maintainer email="[email protected]">Michal Kapinus</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+   <build_depend>pcl_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>art_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_export_depend>pcl_ros</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>art_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+
+
+</package>

art_presence_detector/scripts/control_run.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+
+import rospy
+from std_msgs.msg import Bool
+from std_srvs.srv import Trigger, TriggerRequest
+from art_msgs.srv import ProgramIdTrigger, ProgramIdTriggerResponse, ProgramIdTriggerRequest
+
+
+class ControlProgramRun:
+
+    def __init__(self):
+        self.user_presence_sub = rospy.Subscriber('/art/interface/user/present', Bool,
+                                                  self.user_presence_cb)
+        self.pause_program_client = rospy.ServiceProxy("/art/brain/program/pause", Trigger)
+        self.resume_program_client = rospy.ServiceProxy("/art/brain/program/resume", Trigger)
+        self.stop_program_client = rospy.ServiceProxy("/art/brain/program/stop", Trigger)
+        self.start_program_client = rospy.ServiceProxy("/art/brain/program/start", ProgramIdTrigger)
+        self.time_user_appeared = None
+        self.time_user_disappeared = None
+
+        self.demo_program = rospy.get_param("~demo_program", 1)
+
+        resp = self.start_program_client.call(ProgramIdTriggerRequest(program_id=self.demo_program))
+        if not resp.success:
+            rospy.logerr("Demo program could not be launched, exiting!")
+            rospy.signal_shutdown("Demo program could not be launched")
+        self.program_is_running = True
+        self.program_is_paused = False
+
+    def user_presence_cb(self, data):
+        if data.data:  # user is presented
+            self.time_user_disappeared = None
+            if self.time_user_appeared is None:  # user just arrived
+                self.time_user_appeared = rospy.Time.now()
+                if not self.program_is_paused:
+                    resp = self.pause_program_client.call(TriggerRequest())
+                    if not resp.success:
+                        rospy.logerr("Failed to pause program!")  # TODO: what to do?
+                    self.program_is_paused = True
+            elif self.program_is_running and (rospy.Time.now() - self.time_user_appeared) > rospy.Duration(2):
+                # user is at least 2 seconds in front of the table -> stop program
+                resp = self.stop_program_client.call(TriggerRequest())
+                if not resp.success:
+                    rospy.logerr("Failed to stop program!")  # TODO: what to do?
+                self.program_is_running = False
+                self.program_is_paused = False
+        else:
+            self.time_user_appeared = None
+            if self.time_user_disappeared is None:
+                self.time_user_disappeared = rospy.Time.now()
+                if self.program_is_paused and self.program_is_running:
+                    resp = self.resume_program_client.call(TriggerRequest())
+                    if not resp.success:
+                        rospy.logerr("Failed to resume program!")  # TODO: what to do?
+                    self.program_is_paused = False
+                elif not self.program_is_running:
+                    resp = self.start_program_client.call(ProgramIdTriggerRequest(program_id=self.demo_program))
+                    if not resp.success:
+                        rospy.logerr("Demo program could not be launched!")
+                    self.program_is_running = True
+                    self.program_is_paused = False
+            elif (rospy.Time.now() - self.time_user_disappeared) > rospy.Duration(2) and not self.program_is_running:
+                resp = self.start_program_client.call(ProgramIdTriggerRequest(program_id=self.demo_program))
+                if not resp.success:
+                    rospy.logerr("Demo program could not be launched!")
+                self.program_is_running = True
+                self.program_is_paused = False
+
+
+if __name__ == '__main__':
+    rospy.init_node('control_program_run')
+    ''',log_level=rospy.DEBUG'''
+
+    try:
+        node = ControlProgramRun()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

art_presence_detector/src/node.cpp

@@ -0,0 +1,191 @@
+#include <ros/ros.h>
+// PCL specific includes
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/Image.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/search/search.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/filters/extract_indices.h>
+#include <tf/transform_listener.h>
+#include <pcl/segmentation/region_growing_rgb.h>
+#include <pcl/common/centroid.h>
+#include <pcl/filters/voxel_grid.h>
+
+#include <visualization_msgs/Marker.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <geometry_msgs/PointStamped.h>
+#include <art_msgs/ObjectsCentroids.h>
+#include <std_msgs/Bool.h>
+
+
+
+typedef pcl::PointXYZRGB PointType;
+typedef pcl::PointCloud<PointType> PointCloud;
+typedef PointCloud::Ptr PointCloudPtr;
+
+class ClusterDetector {
+public:
+    ClusterDetector() : spinner(4){
+        sub = nh.subscribe ("/kinect2/qhd/points", 1, &ClusterDetector::cloud_cb, this);
+
+        // Create a ROS publisher for the output point cloud
+        pub = nh.advertise<sensor_msgs::PointCloud2> ("/output", 1);
+        vis_pub = nh.advertise<visualization_msgs::Marker> ("/visualization", 1);
+        user_pub = nh.advertise<std_msgs::Bool> ("/art/interface/user/present", 1);
+        spinner.start();
+    }
+
+private:
+    ros::Publisher pub;
+    ros::Publisher user_pub;
+    ros::Publisher vis_pub;
+    ros::Subscriber sub;
+    tf::TransformListener listener_;
+    ros::NodeHandle nh;
+    ros::AsyncSpinner spinner;
+
+
+
+    void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+    {
+        ros::Time begin = ros::Time::now();
+
+        ROS_INFO_STREAM_ONCE("First point cloud arrived");
+        // Create a container for the data.
+
+        sensor_msgs::PointCloud2 output;
+        pcl::PCLPointCloud2 temp_cloud;
+        pcl::IndicesPtr indices1 (new std::vector <int>), indices2 (new std::vector <int>);
+        pcl_conversions::toPCL(*input, temp_cloud);
+        //temp_cloud.header.stamp = ros::Time::now().toSec()*1000000;
+        PointCloudPtr pc(new PointCloud);
+        pcl::fromPCLPointCloud2(temp_cloud, *pc);
+        PointCloudPtr pc_transformed(new PointCloud);
+
+
+        listener_.waitForTransform(input->header.frame_id, "/marker",
+                                  input->header.stamp, ros::Duration(2));
+
+        pcl_ros::transformPointCloud("/marker", *pc, *pc_transformed, listener_);
+
+
+        pcl::search::Search <PointType>::Ptr tree = boost::shared_ptr<pcl::search::Search<PointType> > (new pcl::search::KdTree<PointType>);
+
+        std::vector<pcl::PointIndices> cluster_indices;
+        pcl::EuclideanClusterExtraction<PointType> ec;
+        PointCloudPtr pc_filtered(new PointCloud);
+
+        pcl::PassThrough<PointType> pass;
+        pass.setInputCloud(pc_transformed);
+        pass.setFilterFieldName("x");
+        pass.setFilterLimits(-0.2,1.2);
+        pass.filter(*indices1);
+
+        pass.setIndices(indices1);
+        pass.setFilterFieldName("y");
+        pass.setFilterLimits(-2,0);
+        pass.filter(*indices2);
+
+        pass.setIndices(indices2);
+        pass.setFilterFieldName("z");
+        pass.setFilterLimits(-1,1);
+        pass.filter(*indices1);
+
+
+
+        pcl::ExtractIndices<PointType> extract;
+        extract.setInputCloud(pc);
+        extract.setIndices(indices1);
+        extract.filter(*pc_filtered);
+
+        pcl::VoxelGrid<PointType> sor;
+        sor.setInputCloud(pc_filtered);
+        sor.setLeafSize (0.01f, 0.01f, 0.01f);
+        sor.filter (*pc_filtered);
+
+
+        ec.setClusterTolerance (0.05);
+        ec.setMinClusterSize (100);
+        ec.setMaxClusterSize (10500);
+        ec.setSearchMethod (tree);
+        ec.setInputCloud (pc_filtered);
+        ec.extract (cluster_indices);
+
+        art_msgs::ObjectsCentroids centroids;
+        centroids.header.frame_id = "/marker";
+
+        std::cout << "Clusters: " << cluster_indices.size() << std::endl;
+        if (cluster_indices.size() > 0) {
+            user_pub.publish(true);
+        } else {
+            user_pub.publish(false);
+        }
+
+        int j = 0;
+        for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+        {
+
+            Eigen::Vector4f centroid;
+            pcl::compute3DCentroid(*pc_filtered, it->indices, centroid);
+            visualization_msgs::Marker marker;
+            marker.header.frame_id = "/kinect2_ir_optical_frame";
+            marker.header.stamp = ros::Time::now();
+            marker.ns = "my_namespace";
+            marker.id = j;
+            marker.type = visualization_msgs::Marker::SPHERE;
+            marker.action = visualization_msgs::Marker::ADD;
+            marker.pose.position.x = centroid[0];
+            marker.pose.position.y = centroid[1];
+            marker.pose.position.z = centroid[2];
+            marker.pose.orientation.x = 0.0;
+            marker.pose.orientation.y = 0.0;
+            marker.pose.orientation.z = 0.0;
+            marker.pose.orientation.w = 1.0;
+            marker.scale.x = 0.03;
+            marker.scale.y = 0.03;
+            marker.scale.z = 0.03;
+            marker.color.a = 1.0; // Don't forget to set the alpha!
+            marker.color.r = 0.0;
+            marker.color.g = 1.0;
+            marker.color.b = 0.0;
+
+            vis_pub.publish( marker );
+            j++;
+
+
+
+
+         }
+
+
+        //ROS_INFO_STREAM("size: " << pc_filtered->size());
+        if (pc_filtered->size() > 0) {
+            pcl::toROSMsg(*pc_filtered, output);
+            pub.publish(output);
+        }
+
+        std::cout << "Processing time: " << ros::Time::now() - begin << std::endl;
+    }
+
+};
+
+
+int main (int argc, char** argv)
+{
+  // Initialize ROS
+  ros::init (argc, argv, "art_cluster_detector_node");
+
+
+  ClusterDetector detector;
+
+  // Create a ROS subscriber for the input point cloud
+
+
+  // Spin
+  ros::waitForShutdown();
+
+}