bool TaskRecorderManagerClient::getInfo(bool &is_recording, ros::Time& first, ros::Time& last, double& sampling_rate)
{
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::GetInfo::Request get_info_request;
task_recorder2::GetInfo::Response get_info_response;
if(!get_info_service_client_.call(get_info_request, get_info_response))
{
ROS_ERROR("Problems when calling >%s<.", get_info_service_client_.getService().c_str());
return false;
}
if(get_info_response.return_code != task_recorder2::GetInfo::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s (%i)", get_info_service_client_.getService().c_str(),
get_info_response.info.c_str(), get_info_response.return_code);
return false;
}
ROS_INFO_STREAM_COND(!get_info_response.info.empty(), get_info_response.info);
is_recording = get_info_response.is_recording;
first = get_info_response.first_recorded_time_stamp;
last = get_info_response.last_recorded_time_stamp;
sampling_rate = get_info_response.sampling_rate;
return true;
}
bool TaskEventDetectorClient::setDescription(const task_recorder2_msgs::Description& description)
{
if(!doesSVMExist(description))
{
ROS_WARN("Check for SVM with description >%s< failed.", task_recorder2_utilities::getFileName(description).c_str());
return (initialized_ = false);
}
task_recorder2_msgs::SetupDetector::Request request;
request.description = description;
task_recorder2_msgs::SetupDetector::Response response;
if (!load_service_client_.call(request, response))
{
ROS_ERROR("Problems when loading SVM for >%s<.", task_recorder2_utilities::getFileName(description).c_str());
return (initialized_ = false);
}
if(response.return_code != task_recorder2_msgs::SetupDetector::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< did not return successfully.", load_service_client_.getService().c_str());
ROS_ERROR_STREAM_COND(!response.info.empty(), response.info);
return false;
}
ROS_INFO_STREAM_COND(!response.info.empty(), response.info);
description_ = description;
return (initialized_ = true);
}
bool TaskRecorderManagerClient::readDataSamples(const task_recorder2_msgs::Description& description,
std::vector<task_recorder2_msgs::DataSample>& data_samples)
{
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::ReadDataSamples::Request read_data_samples_request;
read_data_samples_request.description = description;
task_recorder2::ReadDataSamples::Response read_data_samples_response;
if(!read_data_samples_service_client_.call(read_data_samples_request, read_data_samples_response))
{
ROS_ERROR("Problems when calling >%s<.", read_data_samples_service_client_.getService().c_str());
return false;
}
if(read_data_samples_response.return_code != task_recorder2::ReadDataSamples::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s (%i)", read_data_samples_service_client_.getService().c_str(),
read_data_samples_response.info.c_str(), read_data_samples_response.return_code);
return false;
}
data_samples = read_data_samples_response.data_samples;
ROS_INFO_STREAM_COND(!read_data_samples_response.info.empty(), read_data_samples_response.info);
return true;
}
bool TaskRecorderManagerClient::getDataSample(const task_recorder2_msgs::Description& description,
task_recorder2_msgs::DataSample& data_sample)
{
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::GetDataSample::Request get_data_sample_request;
get_data_sample_request.description = description;
task_recorder2::GetDataSample::Response get_data_sample_response;
if(!get_data_sample_service_client_.call(get_data_sample_request, get_data_sample_response))
{
ROS_ERROR("Problems when calling >%s<.", get_data_sample_service_client_.getService().c_str());
return false;
}
if(get_data_sample_response.return_code != task_recorder2::GetDataSample::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s (%i)", get_data_sample_service_client_.getService().c_str(),
get_data_sample_response.info.c_str(), get_data_sample_response.return_code);
return false;
}
data_sample = get_data_sample_response.data_sample;
ROS_INFO_STREAM_COND(!get_data_sample_response.info.empty(), get_data_sample_response.info);
return true;
}
bool TaskRecorderManagerClient::stopRecording(const ros::Time& start_time,
const ros::Time& end_time,
const int num_samples,
const std::vector<std::string>& message_names,
std::vector<task_recorder2_msgs::DataSample>& messages,
const bool stop_recording)
{
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::StopRecording::Request stop_request;
task_recorder2::StopRecording::Response stop_response;
stop_request.crop_start_time = start_time;
stop_request.crop_end_time = end_time;
stop_request.num_samples = num_samples;
stop_request.message_names = message_names;
stop_request.stop_recording = stop_recording;
if(!stop_recording_service_client_.call(stop_request, stop_response))
{
ROS_ERROR("Problems when calling >%s<.", stop_recording_service_client_.getService().c_str());
return false;
}
if(stop_response.return_code != task_recorder2::StopRecording::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s (%i)", stop_recording_service_client_.getService().c_str(),
stop_response.info.c_str(), stop_response.return_code);
return false;
}
messages = stop_response.filtered_and_cropped_messages;
ROS_INFO_STREAM_COND(!stop_response.info.empty(), stop_response.info);
return true;
}
bool TaskRecorderManagerClient::startRecording(const task_recorder2_msgs::Description& description,
ros::Time& start_time)
{
ROS_DEBUG("Start recording description >%s< with id >%i<.", description.description.c_str(), description.id);
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::StartRecording::Request start_request;
start_request.description = description;
task_recorder2::StartRecording::Response start_response;
if(!start_recording_service_client_.call(start_request, start_response))
{
ROS_ERROR("Problems when calling >%s<.", start_recording_service_client_.getService().c_str());
return false;
}
if(start_response.return_code != task_recorder2::StartRecording::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s", start_recording_service_client_.getService().c_str(), start_response.info.c_str());
return false;
}
start_time = start_response.start_time;
ROS_INFO_STREAM_COND(!start_response.info.empty(), start_response.info);
return true;
}
bool TaskRecorderManagerClient::startStreaming()
{
if(!servicesAreReady())
{
waitForServices();
}
task_recorder2::StartStreaming::Request start_request;
task_recorder2::StartStreaming::Response start_response;
if(!start_streaming_service_client_.call(start_request, start_response))
{
ROS_ERROR("Problems when calling >%s<.", start_streaming_service_client_.getService().c_str());
return false;
}
if(start_response.return_code != task_recorder2::StartStreaming::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< was not successful: %s", start_streaming_service_client_.getService().c_str(), start_response.info.c_str());
return false;
}
ROS_INFO_STREAM_COND(!start_response.info.empty(), start_response.info);
return true;
}
bool TaskEventDetectorClient::checkForEvent(task_recorder2_msgs::DataSampleLabel& label,
const bool save_data_samples,
const int num_data_samples)
{
if(!initialized_)
{
ROS_ERROR("Task event detector client is not initialized. You need to specify a description using setDescription.");
return false;
}
if(!task_recorder_manager_client_.checkForServices())
{
ROS_ERROR("Not all task recorder manager services are online. Cannot check for event.");
return false;
}
ROS_VERIFY(task_recorder_manager_client_.startStreaming());
task_recorder2_msgs::DetectedEvents::Request request;
request.description = description_;
request.method = task_recorder2_msgs::DetectedEvents::Request::AVERAGING;
request.num_data_samples = num_data_samples;
request.save_data_samples = save_data_samples;
task_recorder2_msgs::DetectedEvents::Response response;
if (!detect_service_client_.call(request, response))
{
ROS_ERROR("Problems when detecting events.");
return false;
}
if(response.return_code != task_recorder2_msgs::DetectedEvents::Response::SERVICE_CALL_SUCCESSFUL)
{
ROS_ERROR("Service >%s< did not return successfully.", detect_service_client_.getService().c_str());
ROS_ERROR_STREAM_COND(!response.info.empty(), response.info);
return false;
}
ROS_INFO_STREAM_COND(!response.info.empty(), response.info);
ROS_VERIFY(task_recorder_manager_client_.interruptRecording());
label = response.label;
return true;
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "example2" );
ros::NodeHandle n;
const double val = 3.14;
// Basic messages:
ROS_INFO( "My INFO message." );
ROS_INFO( "My INFO message with argument: %f", val );
ROS_INFO_STREAM(
"My INFO stream message with argument: " << val
);
// Named messages:
ROS_INFO_STREAM_NAMED(
"named_msg",
"My named INFO stream message; val = " << val
);
// Conditional messages:
ROS_INFO_STREAM_COND(
val < 0.,
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND(
val >= 0.,
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Conditional Named messages:
ROS_INFO_STREAM_COND_NAMED(
val < 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_COND_NAMED(
val >= 0., "cond_named_msg",
"My conditional INFO stream message; val (" << val << ") >= 0"
);
// Filtered messages:
struct MyLowerFilter : public ros::console::FilterBase {
MyLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;
}
double value;
};
struct MyGreaterEqualFilter : public ros::console::FilterBase {
MyGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.;
}
double value;
};
MyLowerFilter filter_lower( val );
MyGreaterEqualFilter filter_greater_equal( val );
ROS_INFO_STREAM_FILTER(
&filter_lower,
"My filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"My filter INFO stream message; val (" << val << ") >= 0"
);
// Once messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_ONCE(
"My once INFO stream message; i = " << i
);
}
// Throttle messages:
for( int i = 0; i < 10; ++i ) {
ROS_INFO_STREAM_THROTTLE(
2,
"My throttle INFO stream message; i = " << i
);
ros::Duration( 1 ).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
