bool StartLineProcessConnector::createReader() {
DDSSubscriber *subscriber = RTIConnector::getSubscriber();
if(FAILURE==CControllerThread::getInstance()->CommandWriter("Monitoring_Library","psl_profile"))
{
g_cSystemLogger.LogMessage("(%s:%s:%d):: Error ---> StartLineProcessConnector::CreateWriter Failure\n ", LOG_LOCATION);
}
if (NULL == m_read_topic) {
g_cSystemLogger.LogMessage("(%s:%s:%d):: Error ---> StartLineProcessConnector::CreateReader() m_read_topic is NULL for RTC! \n ", LOG_LOCATION);
return false;
}
if (NULL != m_reader_cache) {
g_cSystemLogger.LogMessage("(%s:%s:%d):: Error ---> RTCConnector::CreateReader() m_reader_cache exists for RTC!\n \n ", LOG_LOCATION);
return false;
}
/* Create a data reader listener for cache */
LineProcessStatusListener *m_reader_listener_cache = new LineProcessStatusListener();
/* To customize the data reader QoS, use the configuration file USER_QOS_PROFILES.xml */
m_reader_cache = subscriber->create_datareader_with_profile(m_read_topic, "Monitoring_Library", "Monitoring_Profile", m_reader_listener_cache,
DDS_STATUS_MASK_ALL);
if (m_reader_cache == NULL) {
g_cSystemLogger.LogMessage("(%s:%s:%d)::CreateReader() error for RTC\n \n ", LOG_LOCATION);
delete m_reader_listener_cache;
m_reader_listener_cache = NULL;
return false;
}
g_cSystemLogger.LogMessage("(%s:%s:%d):: Create Reader sucess", LOG_LOCATION);
return true;
}
void initDataReaders(){
DDSDataReaderSeq temp;
subscriber->get_all_datareaders(temp);
cout<<"Length is << "<<temp.length()<<endl;
DDSDataReader* tempReader;
DDS_DataReaderQos tempQos;
DDS_EntityNameQosPolicy t;
m=new dataReader[temp.length()];
//Load and start the datawriters
for(int i=0;i<temp.length();i++)
{
tempReader=temp.get_at(i);
tempReader->get_qos(tempQos);
t=tempQos.subscription_name;
m[i].initialise_1(subscriber,0,t.name);
}
}
int ACE_TMAIN (int argc, ACE_TCHAR *argv[])
{
DDSTopic* topic = 0;
DDSDataReader* data_reader = 0;
DDSSubscriber* sub = 0;
DDS_ReturnCode_t retcode = DDS_RETCODE_OK;
QueryConditionTestDataReader* typed_dr = 0;
DDSQueryCondition* qc = 0;
DDSReadCondition* rc = 0;
int run = 0;
// NDDS_Config_LogVerbosity n_verbosity =
// static_cast <NDDS_Config_LogVerbosity> (3);
// NDDSConfigLogger::get_instance()->set_verbosity (n_verbosity);
const ACE_TCHAR * env_domain_id = 0;
if (argc > 1)
{
env_domain_id = argv[1];
}
if (!env_domain_id)
{
printf ("Environment variable DEFAULT_DOMAIN_ID not set "
"=> setting it to 2\n");
env_domain_id = "2";
}
else
printf ("Domain ID set to %s\n", env_domain_id);
const int domain_id = ACE_OS::atoi (env_domain_id);
DDSDomainParticipant *participant = DDSDomainParticipantFactory::get_instance()->
create_participant_with_profile (domain_id, /* Domain ID */
LIBRARY_NAME, /* QoS */
PROFILE_NAME,
0, /* Listener */
DDS_STATUS_MASK_NONE);
if (!participant) {
cerr << "RECEIVER: Error creating participant" << endl;
return 1;
}
retcode = QueryConditionTestTypeSupport::register_type(
participant, QueryConditionTestTypeSupport::get_type_name ());
if (retcode != DDS_RETCODE_OK) {
cerr << "RECEIVER: Error registering type" << endl;
return clean_up (participant);
}
topic = participant->create_topic_with_profile (
"QC", /* Topic name*/
QueryConditionTestTypeSupport::get_type_name (), /* Type name */
LIBRARY_NAME, /* QoS */
PROFILE_NAME,
0, /* Listener */
DDS_STATUS_MASK_NONE);
if (!topic) {
cerr << "RECEIVER: Error creating topic" << endl;
return clean_up (participant);
}
sub = participant->create_subscriber_with_profile (LIBRARY_NAME,
PROFILE_NAME,
0,
0);
if (!sub) {
cerr << "RECEIVER: Error creating subscriber" << endl;
return clean_up (participant);
}
data_reader = sub->create_datareader_with_profile (
topic,
LIBRARY_NAME, /* QoS */
PROFILE_NAME,
0, /* Listener */
DDS_STATUS_MASK_NONE);
if (!data_reader) {
cerr << "RECEIVER: Error creating data reader" << endl;
return clean_up (participant);
}
typed_dr = QueryConditionTestDataReader::narrow (data_reader);
if (!typed_dr) {
cerr << "RECEIVER: Unable to cast to a type specific data reader" << endl;
return clean_up (participant);
}
const char* PARAMS_RUN_1[] = {"1", "3"};
DDS_StringSeq parameters_run_1;
parameters_run_1.from_array (PARAMS_RUN_1, 2);
qc = typed_dr->create_querycondition (
DDS_NOT_READ_SAMPLE_STATE,
DDS_NEW_VIEW_STATE | DDS_NOT_NEW_VIEW_STATE,
DDS_ALIVE_INSTANCE_STATE | DDS_NOT_ALIVE_INSTANCE_STATE,
"iteration > %0 AND iteration < %1",
parameters_run_1);
if (!qc) {
cerr << "RECEIVER: Error creating query condition" << endl;
return clean_up (participant);
}
else
cout << "RECEIVER: query condition created : iteration > 1 AND iteration < 3" << endl;
ws_->attach_condition (qc);
cout << "RECEIVER: Expecting two samples (key_1 and key_2) with iteration 2." <<endl;
read (data_reader, qc, rc, ++run);
//Second run: change the parameters
if (qc)
{
const char* PARAMS_RUN_2[] = {"4", "6"};
DDS_StringSeq parameters_run_2;
parameters_run_2.from_array (PARAMS_RUN_2, 2);
if (qc->set_query_parameters (parameters_run_2) != DDS_RETCODE_OK)
{
cerr << "RECEIVER: Unable the set the new query parameters!!!" << endl;
return clean_up (participant);
}
}
cout << "RECEIVER: query condition changed : iteration > 4 AND iteration < 6" << endl;
cout << "RECEIVER: Expecting two samples (key_1 and key_2) with iterations 5" <<endl;
read (data_reader, qc, rc, ++run);
// Third run: Detach querycondition and create read condition instead.
// Remove the qc from the waitset and create a readcondition and start receiving
// samples
if (ws_->detach_condition (qc) != DDS_RETCODE_OK)
{
cerr << "RECEIVER: Error detaching query condition" << endl;
return clean_up (participant);
}
// Delete the query condition from the data reader
typed_dr->delete_readcondition (qc);
cout << "RECEIVER: query condition deleted" << endl;
cout << "RECEIVER: create read condition : DDS_NOT_READ_SAMPLE_STATE,"
<< "DDS_NEW_VIEW_STATE | DDS_NOT_NEW_VIEW_STATE,"
<< "DDS_ALIVE_INSTANCE_STATE | DDS_NOT_ALIVE_INSTANCE_STATE" << endl;
sleep_now(10);
rc = typed_dr->create_readcondition (
DDS_NOT_READ_SAMPLE_STATE,
DDS_NEW_VIEW_STATE | DDS_NOT_NEW_VIEW_STATE,
DDS_ALIVE_INSTANCE_STATE | DDS_NOT_ALIVE_INSTANCE_STATE);
if (!rc) {
cerr << "RECEIVER: Error creating read condition" << endl;
return clean_up (participant);
}
if (ws_->attach_condition (rc) != DDS_RETCODE_OK)
{
cerr << "RECEIVER: Error attaching read condition" << endl;
return clean_up (participant);
}
cout << "RECEIVER: Expecting ALL UNREAD samples (key_1 and key_2) with iterations between 1 and 9, except 2 and 5" <<endl;
read (data_reader, qc, rc, ++run);
return clean_up (participant);
}
bool
DdsRosBridge::start(void)
{
// set up DDS
m_ddsParticipant = createDDSParticipant(0, DDS_TRANSPORTBUILTIN_UDPv4);
if (m_ddsParticipant == 0)
{
return false;
}
DDSSubscriber* ddsSubscriber =
m_ddsParticipant->create_subscriber(DDS_SUBSCRIBER_QOS_DEFAULT, 0, DDS_STATUS_MASK_NONE);
if (ddsSubscriber == 0)
{
ROS_ERROR("create_subscriber error");
ddsShutdown(m_ddsParticipant);
return false;
}
DDS_DataReaderQos reader_qos;
DDS_ReturnCode_t retcode = ddsSubscriber->get_default_datareader_qos(reader_qos);
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("get_default_datareader_qos error %d", retcode);
ddsShutdown(m_ddsParticipant);
return false;
}
// use non-strict reliability model
reader_qos.reliability.kind = DDS_RELIABLE_RELIABILITY_QOS;
reader_qos.history.kind = DDS_KEEP_ALL_HISTORY_QOS;
retcode = ddsSubscriber->set_default_datareader_qos(reader_qos);
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("set_default_datareader_qos error %d", retcode);
ddsShutdown(m_ddsParticipant);
return false;
}
// register type before creating topic
const char* type_name = px_comm::DDSImageTypeSupport::get_type_name();
retcode = px_comm::DDSImageTypeSupport::register_type(m_ddsParticipant, type_name);
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("register_type error %d", retcode);
ddsShutdown(m_ddsParticipant);
return false;
}
m_ddsImageReader.resize(k_nCameras);
m_waitset = new DDSWaitSet();
for (int i = 0; i < k_nCameras; ++i)
{
std::ostringstream oss;
oss << "cam" << i;
DDSTopic* topic =
m_ddsParticipant->create_topic(oss.str().c_str(), type_name,
DDS_TOPIC_QOS_DEFAULT, 0, DDS_STATUS_MASK_NONE);
if (topic == 0)
{
ROS_ERROR("create_topic error");
ddsShutdown(m_ddsParticipant);
return false;
}
DDSDataReader* ddsReader =
ddsSubscriber->create_datareader(topic, DDS_DATAREADER_QOS_DEFAULT, 0,
DDS_STATUS_MASK_NONE);
if (ddsReader == 0)
{
ROS_ERROR("create_datareader error");
ddsShutdown(m_ddsParticipant);
return false;
}
DDSStatusCondition* statusCondition = ddsReader->get_statuscondition();
if (statusCondition == 0)
{
ROS_ERROR("get_statuscondition error");
ddsShutdown(m_ddsParticipant);
return false;
}
m_ddsConditionMap.insert(std::make_pair(statusCondition, i));
retcode = statusCondition->set_enabled_statuses(DDS_DATA_AVAILABLE_STATUS);
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("set_enabled_statuses error %d", retcode);
ddsShutdown(m_ddsParticipant);
return false;
}
retcode = m_waitset->attach_condition(statusCondition);
if (retcode != DDS_RETCODE_OK)
{
ROS_ERROR("attach_condition error %d", retcode);
ddsShutdown(m_ddsParticipant);
return false;
}
m_ddsImageReader.at(i) = px_comm::DDSImageDataReader::narrow(ddsReader);
if (m_ddsImageReader.at(i) == 0)
{
ROS_ERROR("DataReader narrow error");
ddsShutdown(m_ddsParticipant);
return false;
}
ROS_INFO("Subscribed to DDS topic: %s", oss.str().c_str());
}
// set up ROS
m_rosImagePublisher.resize(k_nCameras);
m_rosImage.resize(k_nCameras);
for (int i = 0; i < k_nCameras; ++i)
{
std::ostringstream oss;
oss << "cam" << i << "/image_raw";
m_rosImagePublisher.at(i) = m_imageTransport.advertise(oss.str(), 1);
m_rosImage.at(i) = boost::make_shared<sensor_msgs::Image>();
ROS_INFO("Publishing to ROS topic: %s", m_rosImagePublisher.at(i).getTopic().c_str());
}
return true;
}
