diff --git a/cloud_merge/scripts/do_sweep.py b/cloud_merge/scripts/do_sweep.py index c91bbd14..71c25be7 100755 --- a/cloud_merge/scripts/do_sweep.py +++ b/cloud_merge/scripts/do_sweep.py @@ -1,6 +1,5 @@ #! /usr/bin/env python -import roslib; import rospy import actionlib from actionlib_msgs.msg import GoalStatus @@ -10,138 +9,164 @@ from std_msgs.msg import String from cloud_merge.msg import * import threading -import math -import time class SweepServer: - def __init__(self): - rospy.init_node('sweep_action_server') - self.log_pub = rospy.Publisher('/ptu/log', String) - - self.server = actionlib.SimpleActionServer('do_sweep', cloud_merge.msg.SweepAction, self.execute, False) - self.server.register_preempt_callback(self.preemptCallback) - self.server.start() - rospy.loginfo('Sweep action server started') - - self.aborted = False - self.preempted = False - self.preempt_timeout = 0.3 # seconds - self.sweep_timeout = 240 # seconds - - - self.feedback = scitos_ptu.msg.PanTiltFeedback() - self.result = scitos_ptu.msg.PanTiltResult() - self.result.success = True - - self.preempt_lock = threading.Lock() - - - self.ptugoal = scitos_ptu.msg.PanTiltGoal() - - self.client = actionlib.SimpleActionClient("ptu_pan_tilt_metric_map", scitos_ptu.msg.PanTiltAction) - self.client.wait_for_server() - rospy.loginfo('ptu_pan_tilt_metric_map client created') - - def execute(self, goal): - legal_sweep = False - - if goal.type == "complete" : - rospy.loginfo('Complete sweep type: -160 20 160 -30 30 30') - self.ptugoal.pan_start = -160 - self.ptugoal.pan_step = 20 - self.ptugoal.pan_end = 160 - self.ptugoal.tilt_start = -30 - self.ptugoal.tilt_step = 30 - self.ptugoal.tilt_end = 30 - legal_sweep = True - elif goal.type == 'medium': - rospy.loginfo('Medium sweep type: -160 20 160 -30 -30 -30') - self.ptugoal.pan_start = -160 - self.ptugoal.pan_step = 20 - self.ptugoal.pan_end = 160 - self.ptugoal.tilt_start = -30 - self.ptugoal.tilt_step = -30 - self.ptugoal.tilt_end = -30 - legal_sweep = True - elif goal.type == 'short': - rospy.loginfo('Short sweep type: -160 40 160 -30 -30 -30') - self.ptugoal.pan_start = -160 - self.ptugoal.pan_step = 40 - self.ptugoal.pan_end = 160 - self.ptugoal.tilt_start = -30 - self.ptugoal.tilt_step = -30 - self.ptugoal.tilt_end = -30 - legal_sweep = True - elif goal.type == 'shortest': - rospy.loginfo('Shortest sweep type: -140 60 160 -30 -30 -30') - self.ptugoal.pan_start = -160 - self.ptugoal.pan_step = 60 - self.ptugoal.pan_end = 140 - self.ptugoal.tilt_start = -30 - self.ptugoal.tilt_step = -30 - self.ptugoal.tilt_end = -30 - legal_sweep = True - else: - rospy.loginfo('Unknown sweep type') - - self.preempted = False - self.aborted = False - if legal_sweep: - self._doSweep() - - if self._get_preempt_status(): - self.result.success = False - self.server.set_preempted(self.result) - - elif self.aborted: - self.result.success = False - self.server.set_aborted(self.result) - elif legal_sweep: - if self.client.get_state() == GoalStatus.SUCCEEDED : - self.result.success = True - self.server.set_succeeded(self.result) - else : + + def __init__(self): + rospy.init_node('sweep_action_server') + self.log_pub = rospy.Publisher('/ptu/log', String) + + self.server = actionlib.SimpleActionServer( + 'do_sweep', cloud_merge.msg.SweepAction, self.execute, False) + self.server.register_preempt_callback(self.preemptCallback) + self.server.start() + rospy.loginfo('Sweep action server started') + + self.aborted = False + self.preempted = False + self.preempt_timeout = 0.3 # seconds + self.sweep_timeout = 240 # seconds + + self.feedback = scitos_ptu.msg.PanTiltFeedback() + self.result = scitos_ptu.msg.PanTiltResult() + self.result.success = True + + self.preempt_lock = threading.Lock() + + self.ptugoal = scitos_ptu.msg.PanTiltGoal() + + self.client = actionlib.SimpleActionClient( + "ptu_pan_tilt_metric_map", scitos_ptu.msg.PanTiltAction) + self.client.wait_for_server() + rospy.loginfo('ptu_pan_tilt_metric_map client created') + + def execute(self, goal): + legal_sweep = False + + if goal.type == "complete": + rospy.loginfo('Complete sweep type: -160 20 160 -30 30 30') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 20 + self.ptugoal.pan_end = 160 + self.ptugoal.tilt_start = -30 + self.ptugoal.tilt_step = 30 + self.ptugoal.tilt_end = 30 + legal_sweep = True + elif goal.type == 'medium': + rospy.loginfo('Medium sweep type: -160 20 160 -30 -30 -30') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 20 + self.ptugoal.pan_end = 160 + self.ptugoal.tilt_start = -30 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = -30 + legal_sweep = True + elif goal.type == 'short': + rospy.loginfo('Short sweep type: -160 40 160 -30 -30 -30') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 40 + self.ptugoal.pan_end = 160 + self.ptugoal.tilt_start = -30 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = -30 + legal_sweep = True + elif goal.type == 'shortest': + rospy.loginfo('Shortest sweep type: -160 60 140 -30 -30 -30') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 60 + self.ptugoal.pan_end = 140 + self.ptugoal.tilt_start = -30 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = -30 + legal_sweep = True + elif goal.type == 'middle': + rospy.loginfo('Middle sweep type: -160 20 160 0 -30 0') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 20 + self.ptugoal.pan_end = 160 + self.ptugoal.tilt_start = 0 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = 0 + legal_sweep = True + elif goal.type == 'middle_short': + rospy.loginfo('Middle sweep type: -160 40 160 0 -30 0') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 40 + self.ptugoal.pan_end = 160 + self.ptugoal.tilt_start = 0 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = 0 + legal_sweep = True + elif goal.type == 'middle_shortest': + rospy.loginfo('Middle sweep type: -160 60 140 0 -30 0') + self.ptugoal.pan_start = -160 + self.ptugoal.pan_step = 60 + self.ptugoal.pan_end = 140 + self.ptugoal.tilt_start = 0 + self.ptugoal.tilt_step = -30 + self.ptugoal.tilt_end = 0 + legal_sweep = True + else: + rospy.loginfo('Unknown sweep type') + + self.preempted = False + self.aborted = False + if legal_sweep: + self._doSweep() + + if self._get_preempt_status(): self.result.success = False - self.server.set_succeeded(self.result) - else: - self.result.success = False - self.server.set_succeeded(self.result) + self.server.set_preempted(self.result) + elif self.aborted: + self.result.success = False + self.server.set_aborted(self.result) + elif legal_sweep: + if self.client.get_state() == GoalStatus.SUCCEEDED: + self.result.success = True + self.server.set_succeeded(self.result) + else: + self.result.success = False + self.server.set_succeeded(self.result) + else: + self.result.success = False + self.server.set_succeeded(self.result) - def _doSweep(self): + def _doSweep(self): time_waited = 0 self.client.send_goal(self.ptugoal) self.client.wait_for_result(rospy.Duration(self.preempt_timeout)) - status= self.client.get_state() + status = self.client.get_state() while not status == GoalStatus.SUCCEEDED and not time_waited > self.sweep_timeout and not self._get_preempt_status(): - # figure out how to get the feedback out of the client -# feedback = self.client.get_feedback() -# self.server.publish_feedback(feedback) - time_waited += self.preempt_timeout - self.client.wait_for_result(rospy.Duration(self.preempt_timeout)) - status= self.client.get_state() + # figure out how to get the feedback out of the client + # feedback = self.client.get_feedback() + # self.server.publish_feedback(feedback) + time_waited += self.preempt_timeout + self.client.wait_for_result(rospy.Duration(self.preempt_timeout)) + status = self.client.get_state() if self._get_preempt_status(): - # this action server has been preempted; preempt the other one as well - self.client.cancel_goal() + # this action server has been preempted; preempt the other one as + # well + self.client.cancel_goal() elif time_waited > self.sweep_timeout or status != GoalStatus.SUCCEEDED: - # didn't manage to reach the PTU position - self.client.cancel_goal() - self.aborted = True + # didn't manage to reach the PTU position + self.client.cancel_goal() + self.aborted = True - def _get_preempt_status(self): - self.preempt_lock.acquire() - preempted = self.preempted - self.preempt_lock.release() - return preempted + def _get_preempt_status(self): + self.preempt_lock.acquire() + preempted = self.preempted + self.preempt_lock.release() + return preempted - def preemptCallback(self): - self.preempt_lock.acquire() - self.preempted = True - self.preempt_lock.release() + def preemptCallback(self): + self.preempt_lock.acquire() + self.preempted = True + self.preempt_lock.release() if __name__ == '__main__': - server = SweepServer() - rospy.spin() + server = SweepServer() + rospy.spin() diff --git a/metaroom_xml_parser/CMakeLists.txt b/metaroom_xml_parser/CMakeLists.txt index 05cf26ba..1a5e8345 100644 --- a/metaroom_xml_parser/CMakeLists.txt +++ b/metaroom_xml_parser/CMakeLists.txt @@ -77,7 +77,7 @@ add_executable(load_labelled_data src/load_labelled_data.cpp ) ############################# INSTALL TARGETS -install(TARGETS metaroom_xml_parser load_single_file load_multiple_files +install(TARGETS metaroom_xml_parser load_single_file load_multiple_files load_labelled_data ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/metaroom_xml_parser/README.md b/metaroom_xml_parser/README.md index edd437ec..5c31043a 100644 --- a/metaroom_xml_parser/README.md +++ b/metaroom_xml_parser/README.md @@ -57,6 +57,8 @@ The intermediate cloud datatype is: std::vector>> vIntermediateRoomClouds; std::vector vIntermediateRoomCloudTransforms; std::vector vIntermediateRoomCloudCamParams; + std::vector vIntermediateRoomCloudTransformsRegistered; + std::vector vIntermediateRoomCloudCamParamsCorrected; std::vector vIntermediateRGBImages; // type CV_8UC3 std::vector vIntermediateDepthImages; // type CV_16UC1 }; diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.h b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.h index f607fee8..92cba283 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.h +++ b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.h @@ -28,6 +28,8 @@ namespace semantic_map_load_utilties std::vector>> vIntermediateRoomClouds; std::vector vIntermediateRoomCloudTransforms; std::vector vIntermediateRoomCloudCamParams; + std::vector vIntermediateRoomCloudTransformsRegistered; + std::vector vIntermediateRoomCloudCamParamsCorrected; std::vector vIntermediateRGBImages; // type CV_8UC3 std::vector vIntermediateDepthImages; // type CV_16UC1 }; @@ -87,7 +89,10 @@ namespace semantic_map_load_utilties tf::StampedTransform transformToGlobal; tf::Vector3 sweepCenter; std::vector>> objectClouds; + std::vector objectImages; + std::vector objectMasks; std::vector objectLabels; + std::vector objectScanIndices; boost::posix_time::ptime sweepTime; std::string waypoint; @@ -117,4 +122,3 @@ namespace semantic_map_registration_features } #endif - diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp index 4c4dc3d6..4a7bf7c1 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp +++ b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp @@ -72,6 +72,8 @@ toRet.vIntermediateRoomClouds = sweep.vIntermediateRoomClouds; toRet.vIntermediateRoomCloudTransforms = sweep.vIntermediateRoomCloudTransforms; toRet.vIntermediateRoomCloudCamParams = sweep.vIntermediateRoomCloudCamParams; + toRet.vIntermediateRoomCloudTransformsRegistered = sweep.vIntermediateRoomCloudTransformsRegistered; + toRet.vIntermediateRoomCloudCamParamsCorrected = sweep.vIntermediateRoomCloudCamParamsCorrected; toRet.vIntermediateRGBImages = sweep.vIntermediateRGBImages; toRet.vIntermediateDepthImages = sweep.vIntermediateDepthImages; @@ -306,7 +308,7 @@ sort(matchingObservations.begin(), matchingObservations.end(), [](const std::string& a, const std::string& b ) - { + { std::string patrol_string = "patrol_run_"; std::string room_string = "room_"; std::string date_string = "YYYYMMDD"; @@ -467,9 +469,21 @@ unsigned found = sweepXmlPath.find_last_of("/"); std::string base_path = sweepXmlPath.substr(0,found+1); QStringList xmlFiles = QDir(base_path.c_str()).entryList(QStringList("*label*.pcd")); + QStringList imageFiles = QDir(base_path.c_str()).entryList(QStringList("*object*.jpg")); + QStringList maskFiles = QDir(base_path.c_str()).entryList(QStringList("*label*.jpg")); + + if (xmlFiles.size() != imageFiles.size()) + { + ROS_INFO_STREAM("In " << sweepXmlPath << " found different number of labels and object images."); + } for (size_t k=0; k 200; + toRet.objectClouds.push_back(cloud); + toRet.objectImages.push_back(image); + toRet.objectMasks.push_back(mask); toRet.objectLabels.push_back(label); + toRet.objectScanIndices.push_back(scan_number); } return toRet; } - diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h index 1293055a..2c3a6fff 100644 --- a/object_manager/include/object_manager/object_manager.h +++ b/object_manager/include/object_manager/object_manager.h @@ -44,7 +44,7 @@ #include #include #include - +#include #include // application includes @@ -125,6 +125,7 @@ class ObjectManager { std::map m_waypointToSweepFileMap; bool m_bLogToDB; bool m_bTrackingStarted; + bool m_bSaveMask; DynamicObject::Ptr m_objectTracked; std::string m_objectTrackedObservation; // for saving int m_MinClusterSize; @@ -159,6 +160,15 @@ ObjectManager::ObjectManager(ros::NodeHandle nh) : m_TransformListene } else { ROS_INFO_STREAM("The dynamic objects will NOT be logged to the database."); } + m_NodeHandle.param("save_mask",m_bSaveMask,true); + if (m_bLogToDB) + { + ROS_INFO_STREAM("The dynamic object mask (image and indices) will be saved on the disk."); + } else { + ROS_INFO_STREAM("The dynamic object mask (image and indices) will NOT be saved on the disk."); + } + + m_NodeHandle.param("min_object_size",m_MinClusterSize,500); ROS_INFO_STREAM("ObjectManager:: min object size set to "<::returnObjectMask(std::string waypoint, std::strin return false; } + // reproject intermediate cloud using original camera parameters + auto camParamOrig = observation.getIntermediateCloudCameraParameters(); + double fx = camParamOrig[0].fx(); + double fy = camParamOrig[0].fy(); + double center_x = camParamOrig[0].cx(); + double center_y = camParamOrig[0].cy(); + double cx = 0.001 / fx; + double cy = 0.001 / fy; + + CloudPtr reprojected_cloud(new Cloud); + RegistrationFeatures reg; + + std::pair rgbAndDepth = reg.createRGBandDepthFromPC(transformedCloud); + + pcl::PointXYZRGB point; + for (size_t y = 0; y < rgbAndDepth.first.rows; ++y) { + for (size_t x = 0; x < rgbAndDepth.first.cols; ++x) { + + uint16_t depth = rgbAndDepth.second.at(y, x)/* * 1000*/; + if (!(depth != 0)) + { + point.x = point.y = point.z = std::numeric_limits::quiet_NaN(); + } else { + point.x = (x - center_x) * depth * cx; + point.y = (y - center_y) * depth * cy; + point.z = depth * 0.001f; + } + uint32_t rgb = ((uint8_t)rgbAndDepth.first.at(y, x)[2] << 16 | rgbAndDepth.first.at(y, x)[1] << 8 | rgbAndDepth.first.at(y, x)[0]); + point.rgb = *reinterpret_cast(&rgb); + + reprojected_cloud->points[y*rgbAndDepth.first.cols + x] = point; + } + } + *transformedCloud = *reprojected_cloud; + cv::Mat cluster_image = cv::Mat::zeros(480, 640, CV_8UC3); int top_y = -1, bottom_y = 640, top_x = -1, bottom_x = 640; for (int index : src_indices) @@ -775,6 +820,7 @@ bool ObjectManager::returnObjectMask(std::string waypoint, std::strin // cv::imwrite("image.jpg", cluster_image); // cv::imshow( "Display window", cluster_image ); // Show our image inside it. + const Eigen::Affine3d eigenTr(best_transform.cast()); tf::transformEigenToTF(eigenTr,returned_object.transform_to_map); returned_object.object_cloud = CloudPtr(new Cloud()); @@ -784,6 +830,24 @@ bool ObjectManager::returnObjectMask(std::string waypoint, std::strin returned_object.object_indices.insert(returned_object.object_indices.begin(), src_indices.begin(),src_indices.end()); returned_object.object_mask = cluster_image; + // save mask and indices + if (m_bSaveMask) { + // find observation folder + int slash_pos = observation_xml.find_last_of("/"); + std::string observation_folder = observation_xml.substr(0, slash_pos); + std::string mask_image = observation_folder + "/" + object_id + "_mask.jpg"; + cv::imwrite(mask_image, cluster_image); + std::string mask_indices = observation_folder + "/" + object_id + "_mask.txt"; + ofstream mask_indices_os; + mask_indices_os.open(mask_indices); + for (int index : returned_object.object_indices){ + mask_indices_os << index<<" "; + } + mask_indices_os.close(); + ROS_INFO_STREAM("Object mask saved at: "< + @@ -53,6 +54,7 @@ +