diff --git a/art_arcode_detector/bundles/blue_container.xml b/art_arcode_detector/bundles/blue_container.xml new file mode 100644 index 0000000..b531b00 --- /dev/null +++ b/art_arcode_detector/bundles/blue_container.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/art_presence_detector/CMakeLists.txt b/art_presence_detector/CMakeLists.txt new file mode 100644 index 0000000..8469245 --- /dev/null +++ b/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} +) diff --git a/art_presence_detector/launch/art_presence_detector.launch b/art_presence_detector/launch/art_presence_detector.launch new file mode 100644 index 0000000..54581f4 --- /dev/null +++ b/art_presence_detector/launch/art_presence_detector.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/art_presence_detector/package.xml b/art_presence_detector/package.xml new file mode 100644 index 0000000..9ddd60a --- /dev/null +++ b/art_presence_detector/package.xml @@ -0,0 +1,28 @@ + + + art_presence_detector + 0.0.0 + The art_presence_detector package + + Michal Kapinus + + TODO + + catkin + pcl_ros + roscpp + art_msgs + std_msgs + sensor_msgs + pcl_conversions + pcl_ros + roscpp + pcl_conversions + pcl_ros + sensor_msgs + art_msgs + std_msgs + roscpp + + + diff --git a/art_presence_detector/scripts/control_run.py b/art_presence_detector/scripts/control_run.py new file mode 100755 index 0000000..706034a --- /dev/null +++ b/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 diff --git a/art_presence_detector/src/node.cpp b/art_presence_detector/src/node.cpp new file mode 100644 index 0000000..4aec52d --- /dev/null +++ b/art_presence_detector/src/node.cpp @@ -0,0 +1,191 @@ +#include +// PCL specific includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + + + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud 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 ("/output", 1); + vis_pub = nh.advertise ("/visualization", 1); + user_pub = nh.advertise ("/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 ), indices2 (new std::vector ); + 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 ::Ptr tree = boost::shared_ptr > (new pcl::search::KdTree); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + PointCloudPtr pc_filtered(new PointCloud); + + pcl::PassThrough 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 extract; + extract.setInputCloud(pc); + extract.setIndices(indices1); + extract.filter(*pc_filtered); + + pcl::VoxelGrid 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::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(); + +}