Skip to content

Commit

Permalink
Merge pull request #84 from RaresAmbrus/hydro-devel
Browse files Browse the repository at this point in the history 
pre ICRA changes and an install target fix
  • Loading branch information
@marc-hanheide
marc-hanheide committed Dec 2, 2015
2 parents 6471732 + bb99d39 commit a6d4ebd
Show file tree
Hide file tree
Showing 7 changed files with 242 additions and 123 deletions.
261 changes: 143 additions & 118 deletions cloud_merge/scripts/do_sweep.py
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
#! /usr/bin/env python

import roslib;
import rospy
import actionlib
from actionlib_msgs.msg import GoalStatus
Expand All @@ -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()
2 changes: 1 addition & 1 deletion metaroom_xml_parser/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}
Expand Down
2 changes: 2 additions & 0 deletions metaroom_xml_parser/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,8 @@ The intermediate cloud datatype is:
std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>> vIntermediateRoomClouds;
std::vector<tf::StampedTransform> vIntermediateRoomCloudTransforms;
std::vector<image_geometry::PinholeCameraModel> vIntermediateRoomCloudCamParams;
std::vector<tf::StampedTransform> vIntermediateRoomCloudTransformsRegistered;
std::vector<image_geometry::PinholeCameraModel> vIntermediateRoomCloudCamParamsCorrected;
std::vector<cv::Mat> vIntermediateRGBImages; // type CV_8UC3
std::vector<cv::Mat> vIntermediateDepthImages; // type CV_16UC1
};
Expand Down
6 changes: 5 additions & 1 deletion metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ namespace semantic_map_load_utilties
std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>> vIntermediateRoomClouds;
std::vector<tf::StampedTransform> vIntermediateRoomCloudTransforms;
std::vector<image_geometry::PinholeCameraModel> vIntermediateRoomCloudCamParams;
std::vector<tf::StampedTransform> vIntermediateRoomCloudTransformsRegistered;
std::vector<image_geometry::PinholeCameraModel> vIntermediateRoomCloudCamParamsCorrected;
std::vector<cv::Mat> vIntermediateRGBImages; // type CV_8UC3
std::vector<cv::Mat> vIntermediateDepthImages; // type CV_16UC1
};
Expand Down Expand Up @@ -87,7 +89,10 @@ namespace semantic_map_load_utilties
tf::StampedTransform transformToGlobal;
tf::Vector3 sweepCenter;
std::vector<boost::shared_ptr<pcl::PointCloud<PointType>>> objectClouds;
std::vector<cv::Mat> objectImages;
std::vector<cv::Mat> objectMasks;
std::vector<std::string> objectLabels;
std::vector<size_t> objectScanIndices;
boost::posix_time::ptime sweepTime;
std::string waypoint;

Expand Down Expand Up @@ -117,4 +122,3 @@ namespace semantic_map_registration_features
}

#endif

26 changes: 24 additions & 2 deletions metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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<xmlFiles.size(); k++)
{
// get the frame number of the label
std::string label_name = xmlFiles[k].toStdString();
std::string number_string = label_name.substr(4, 4);
size_t scan_number = std::stoi(number_string);

std::string label_file = base_path+xmlFiles[k].toStdString();
label_file[label_file.size()-1] = 't';
label_file[label_file.size()-2] = 'x';
Expand All @@ -487,10 +501,18 @@
continue;
}

cv::Mat image = cv::imread(base_path+imageFiles[k].toStdString());
cv::Mat mask_color = cv::imread(base_path+maskFiles[k].toStdString());
cv::Mat mask;
cv::cvtColor(mask_color, mask, CV_BGR2GRAY);
mask = mask > 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;
}

Loading

0 comments on commit a6d4ebd

Please sign in to comment.