OpenNI2Driver::OpenNI2Driver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
nh_(n),
pnh_(pnh),
device_manager_(OpenNI2DeviceManager::getSingelton()),
config_init_(false),
data_skip_ir_counter_(0),
data_skip_color_counter_(0),
data_skip_depth_counter_ (0),
ir_subscribers_(false),
color_subscribers_(false),
depth_subscribers_(false),
depth_raw_subscribers_(false)
{
genVideoModeTableMap();
readConfigFromParameterServer();
initDevice();
// Initialize dynamic reconfigure
reconfigure_server_.reset(new ReconfigureServer(pnh_));
reconfigure_server_->setCallback(boost::bind(&OpenNI2Driver::configCb, this, _1, _2));
while (!config_init_)
{
ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
boost::this_thread::sleep(boost::posix_time::seconds(0.1));
}
ROS_DEBUG("Dynamic reconfigure configuration received.");
advertiseROSTopics();
}
AstraDriver::AstraDriver(ros::NodeHandle& n, ros::NodeHandle& pnh) :
nh_(n),
pnh_(pnh),
device_manager_(AstraDeviceManager::getSingelton()),
config_init_(false),
data_skip_ir_counter_(0),
data_skip_color_counter_(0),
data_skip_depth_counter_ (0),
ir_subscribers_(false),
color_subscribers_(false),
depth_subscribers_(false),
depth_raw_subscribers_(false)
{
genVideoModeTableMap();
readConfigFromParameterServer();
#if MULTI_ASTRA
int bootOrder, devnums;
pnh.getParam("bootorder", bootOrder);
pnh.getParam("devnums", devnums);
if( devnums>1 )
{
int shmid;
char *shm = NULL;
char *tmp;
if( bootOrder==1 )
{
if( (shmid = shmget((key_t)0401, 1, 0666|IPC_CREAT)) == -1 )
{
ROS_ERROR("Create Share Memory Error:%s", strerror(errno));
}
shm = (char *)shmat(shmid, 0, 0);
*shm = 1;
initDevice();
ROS_WARN("*********** device_id %s already open device************************ ", device_id_.c_str());
*shm = 2;
}
else
{
if( (shmid = shmget((key_t)0401, 1, 0666|IPC_CREAT)) == -1 )
{
ROS_ERROR("Create Share Memory Error:%s", strerror(errno));
}
shm = (char *)shmat(shmid, 0, 0);
while( *shm!=bootOrder);
initDevice();
ROS_WARN("*********** device_id %s already open device************************ ", device_id_.c_str());
*shm = (bootOrder+1);
}
if( bootOrder==1 )
{
while( *shm!=(devnums+1)) ;
if(shmdt(shm) == -1)
{
ROS_ERROR("shmdt failed\n");
}
if(shmctl(shmid, IPC_RMID, 0) == -1)
{
ROS_ERROR("shmctl(IPC_RMID) failed\n");
}
}
else
{
if(shmdt(shm) == -1)
{
ROS_ERROR("shmdt failed\n");
}
}
}
else
{
initDevice();
}
#else
initDevice();
#endif
// Initialize dynamic reconfigure
reconfigure_server_.reset(new ReconfigureServer(pnh_));
reconfigure_server_->setCallback(boost::bind(&AstraDriver::configCb, this, _1, _2));
while (!config_init_)
{
ROS_DEBUG("Waiting for dynamic reconfigure configuration.");
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
}
ROS_DEBUG("Dynamic reconfigure configuration received.");
advertiseROSTopics();
}