Skip to content

Commit

Permalink
Added more error checks
Browse files Browse the repository at this point in the history
  • Loading branch information
@reaganlo
reaganlo committed Dec 21, 2016
1 parent c282ddc commit 5b01612
Show file tree
Hide file tree
Showing 3 changed files with 79 additions and 54 deletions.
3 changes: 1 addition & 2 deletions include/realsense_person/person_nodelet.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,6 @@ namespace realsense_person

RSCore::projection_interface* projection_interface_;
std::unique_ptr<PersonModuleInterface::person_tracking_video_module_interface> pt_video_module_;
PersonModule::PersonTrackingData* person_data_; // Always access this variable only using getPersonData()

ros::ServiceServer tracking_id_server_;
ros::ServiceServer register_server_;
Expand Down Expand Up @@ -161,7 +160,7 @@ namespace realsense_person
void imageCallback(const sensor_msgs::ImageConstPtr& color_image, const sensor_msgs::ImageConstPtr& depth_image);
void generateSampleSet(RSCore::stream_type image_type, RSCore::pixel_format image_format,
const sensor_msgs::ImageConstPtr& image, RSCore::correlated_sample_set &sample_set);
void setPersonData(RSCore::correlated_sample_set sample_set);
void processFrame(RSCore::correlated_sample_set sample_set);
PersonModule::PersonTrackingData* getPersonData();
void prepareMsgs(PersonModule::PersonTrackingData* person_data, const sensor_msgs::ImageConstPtr& color_image);
Person preparePersonMsg(int tracking_id, PersonModule::PersonTrackingData::BoundingBox2D b_box,
Expand Down
128 changes: 76 additions & 52 deletions src/person_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -360,7 +360,7 @@ namespace realsense_person
generateSampleSet(RSCore::stream_type::color, RSCore::pixel_format::rgb8, color_image, sample_set);
generateSampleSet(RSCore::stream_type::depth, RSCore::pixel_format::z16, depth_image, sample_set);

setPersonData(sample_set);
processFrame(sample_set);

double now = (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count()
/ 1000.00);
Expand All @@ -371,7 +371,11 @@ namespace realsense_person
{
last_publish_time_ = now;
auto person_data = getPersonData();
if (person_data)
if (!person_data)
{
ROS_WARN_STREAM(nodelet_name_ << " - Could not get person data");
}
else
{
prepareMsgs(person_data, color_image);
}
Expand All @@ -398,24 +402,16 @@ namespace realsense_person
}

/*
* Set Person Data.
* Process Frame.
*/
void PersonNodelet::setPersonData(RSCore::correlated_sample_set sample_set)
void PersonNodelet::processFrame(RSCore::correlated_sample_set sample_set)
{
std::unique_lock<std::mutex> lock(frame_lock_);
auto status = pt_video_module_->process_sample_set_sync(&sample_set);

if (status != RSCore::status::status_no_error)
{
ROS_ERROR_STREAM(nodelet_name_ << " - Error while processing input frames: " << status);
person_data_ = nullptr;
}

person_data_ = pt_video_module_->QueryOutput();

if (!person_data_)
{
ROS_ERROR_STREAM(nodelet_name_ << " - No person data");
}
}

Expand All @@ -425,7 +421,7 @@ namespace realsense_person
PersonModule::PersonTrackingData* PersonNodelet::getPersonData()
{
std::unique_lock<std::mutex> lock(frame_lock_);
return person_data_;
return pt_video_module_->QueryOutput();
}

/*
Expand Down Expand Up @@ -651,16 +647,25 @@ namespace realsense_person
{
ROS_INFO_STREAM(nodelet_name_ << " - Calling service: " << GET_TRACKING_ID_SERVICE);
auto person_data = getPersonData();

int detected_person_cnt = person_data->QueryNumberOfPeople();
res.detected_person_count = detected_person_cnt;
for (int i = 0; i < detected_person_cnt; ++i)
if (!person_data)
{
PersonModule::PersonTrackingData::Person* single_person_data =
person_data->QueryPersonData(PersonModule::PersonTrackingData::ACCESS_ORDER_BY_ID, i);
PersonModule::PersonTrackingData::PersonTracking* detection_data = single_person_data->QueryTracking();
int tracking_id = detection_data->QueryId();
res.tracking_ids.push_back(tracking_id);
res.status = -1;
res.status_desc = "Could not get person data";
}
else
{
res.status = 0;
res.status_desc = "Success";
int detected_person_cnt = person_data->QueryNumberOfPeople();
res.detected_person_count = detected_person_cnt;
for (int i = 0; i < detected_person_cnt; ++i)
{
PersonModule::PersonTrackingData::Person* single_person_data =
person_data->QueryPersonData(PersonModule::PersonTrackingData::ACCESS_ORDER_BY_ID, i);
PersonModule::PersonTrackingData::PersonTracking* detection_data = single_person_data->QueryTracking();
int tracking_id = detection_data->QueryId();
res.tracking_ids.push_back(tracking_id);
}
}
return true;
}
Expand All @@ -676,14 +681,13 @@ namespace realsense_person
if (!person_data)
{
res.status = -1;
res.status_desc = "Could not start tracking";
res.status_desc = "Could not get person_data";
}
else
{
tracking_id_ = req.tracking_id;
pt_video_module_->QueryConfiguration()->QueryTracking()->Enable();
pt_video_module_->QueryConfiguration()->QueryTracking()->EnableFaceLandmarks();
auto person_data = getPersonData();
person_data->StartTracking(tracking_id_);
res.status = 0;
res.status_desc = "Started tracking person with tracking_id " + std::to_string(tracking_id_);
Expand All @@ -702,7 +706,7 @@ namespace realsense_person
if (!person_data)
{
res.status = -1;
res.status_desc = "Could not stop tracking";
res.status_desc = "Could not get person data";
}
else
{
Expand All @@ -725,23 +729,29 @@ namespace realsense_person
if (!pt_video_module_->QueryConfiguration()->QueryRecognition()->IsEnabled())
{
res.status_desc = "Recognition Not Enabled";

}
else
{
auto person_data = getPersonData();
auto person = person_data->QueryPersonDataById(req.tracking_id);

if (!person)
if (!person_data)
{
res.status_desc = "Tracking Id Not Found";
res.status = -1;
res.status_desc = "Could not get person data";
}
else
{
int recognition_id = -1;
res.status = person->QueryRecognition()->RegisterUser(&recognition_id);
res.status_desc = REGISTRATION_DESC[res.status];
res.recognition_id = recognition_id;
auto person = person_data->QueryPersonDataById(req.tracking_id);
if (!person)
{
res.status_desc = "Tracking Id Not Found";
}
else
{
int recognition_id = -1;
res.status = person->QueryRecognition()->RegisterUser(&recognition_id);
res.status_desc = REGISTRATION_DESC[res.status];
res.recognition_id = recognition_id;
}
}
}
return true;
Expand All @@ -764,22 +774,29 @@ namespace realsense_person
else
{
auto person_data = getPersonData();
auto person = person_data->QueryPersonDataById(req.tracking_id);

if (!person)
if (!person_data)
{
res.status = -1;
res.status_desc = "Tracking Id Not Found";
res.recognition_id = -1;
res.confidence = -1.0;
res.status_desc = "Could not get person data";
}
else
{
PersonModule::PersonTrackingData::RecognizerData result;
res.status = person->QueryRecognition()->RecognizeUser(&result);
res.status_desc = RECOGNITION_DESC[res.status];
res.recognition_id = result.recognitionId;
res.confidence = result.similarityScore;
auto person = person_data->QueryPersonDataById(req.tracking_id);
if (!person)
{
res.status = -1;
res.status_desc = "Tracking Id Not Found";
res.recognition_id = -1;
res.confidence = -1.0;
}
else
{
PersonModule::PersonTrackingData::RecognizerData result;
res.status = person->QueryRecognition()->RecognizeUser(&result);
res.status_desc = RECOGNITION_DESC[res.status];
res.recognition_id = result.recognitionId;
res.confidence = result.similarityScore;
}
}
}
return true;
Expand All @@ -800,18 +817,25 @@ namespace realsense_person
else
{
auto person_data = getPersonData();
auto person = person_data->QueryPersonDataById(req.tracking_id);

if (!person)
if (!person_data)
{
res.status = -1;
res.status_desc = "Tracking Id Not Found";
res.status_desc = "Could not get person data";
}
else
{
res.status = person->QueryRecognition()->ReinforceUserRegistration(req.recognition_id,
PersonModule::PersonTrackingData::PersonRecognition::RegisterPolicyManualAdd);
res.status_desc = REGISTRATION_DESC[res.status];
auto person = person_data->QueryPersonDataById(req.tracking_id);
if (!person)
{
res.status = -1;
res.status_desc = "Tracking Id Not Found";
}
else
{
res.status = person->QueryRecognition()->ReinforceUserRegistration(req.recognition_id,
PersonModule::PersonTrackingData::PersonRecognition::RegisterPolicyManualAdd);
res.status_desc = REGISTRATION_DESC[res.status];
}
}
}
return true;
Expand Down
2 changes: 2 additions & 0 deletions srv/GetTrackingId.srv
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@
---
int32 status
string status_desc
uint32 detected_person_count
uint32[] tracking_ids #list of tracking_ids of detected people

0 comments on commit 5b01612

Please sign in to comment.