RuntimeInterface::ErrorCode
SwitchWContexts::serialize(std::ostream *out) {
std::unique_lock<std::mutex> config_lock(config_mutex);
(*out) << serialization_format_version_str << "\n";
std::string md5sum = get_config_md5_();
(*out) << md5sum << "\n";
for (auto &cxt : contexts) {
ErrorCode rc = cxt.serialize(out);
if (rc != ErrorCode::SUCCESS) return rc;
}
return ErrorCode::SUCCESS;
}
static int modbus_after_write_table(struct modbus_instance *instance, enum modbus_table table, uint16_t address, uint16_t count)
{
if (table == MODBUS_TABLE_HOLDING_REGISTERS)
{
uint16_t offset = ELEMENT_OFFSET(common_values, now);
if (ELEMENT_IN(0x0000, offset, address * 2, count * 2))
{
// SYNC CLOCK
DateTime dt(common_values.now.year, common_values.now.month, common_values.now.day,
common_values.now.hours, common_values.now.minutes, common_values.now.seconds);
OS::TSysTimerLocker lock;
RTC_DS1307 rtc;
rtc.begin();
rtc.adjust(dt);
}
offset = ELEMENT_OFFSET(common_values, modbus_address);
if (ELEMENT_IN(0x0000, offset, address * 2, count * 2))
{
config_lock();
config_set_address(common_values.modbus_address & 0xFF);
config_unlock();
// instance->address = common_values.modbus_address;
}
}
if (table == MODBUS_TABLE_COILS)
{
uint16_t offset = ELEMENT_OFFSET(controls, restart_programs);
if (ELEMENT_IN(0x0000, offset, address, count))
{
if (controls.restart_programs == 0x01)
{
controls.restart_programs = 0x00;
controls.pause_flag = 0x00;
program_reset();
config_set_pause_flag(controls.pause_flag);
}
}
offset = ELEMENT_OFFSET(controls, pause_flag);
if (ELEMENT_IN(0x0000, offset, address, count))
{
config_set_pause_flag(controls.pause_flag);
}
}
MODBUS_RETURN(instance, MODBUS_SUCCESS);
}
static int modbus_write_file(struct modbus_instance *instance, uint16_t filenum, uint16_t address, uint16_t count, const uint8_t *data)
{
uint16_t start_address;
bool found = false;
if (filenum == 0x0001)
{
start_address = IOSLOT_START_ADDRESS;
found = true;
}
else if (filenum == 0x0002)
{
start_address = PROGRAM_START_ADDRESS;
found = true;
}
if (found)
{
uint16_t i;
uint16_t j;
config_lock();
for (i = 0, j = 0; i < count; ++i, ++address, j += 2)
{
uint8_t w[2];
w[0] = data[j + 1];
w[1] = data[j];
config_write(start_address + (address << 1), w, 2);
}
config_unlock();
if (filenum == 0x0001)
{
io_reset();
}
else if (filenum == 0x0002)
{
program_reset();
}
MODBUS_RETURN(instance, MODBUS_SUCCESS);
}
MODBUS_RETURN(instance, MODBUS_BAD_PARAMS);
}
void HeaderManipulation::configCB(Config &config, uint32_t level)
{
ROS_INFO("HeaderManipulation configCB");
boost::mutex::scoped_lock config_lock(config_mutex_);
frame_id_new_ = config.frame_id_new;
frame_id_to_replace_ = config.frame_id_to_replace;
msg_delay_ = ros::Duration(config.msg_delay_milliseconds/1000);
publish_retry_rate_ = ros::Rate(config.publish_retry_rate);
time_offset_ = ros::Duration(config.time_offset_milliseconds/1000);
if (frame_id_to_replace_.length() != frame_id_new_.length())
{
ROS_ERROR("frame_id_to_replace and frame_id_new do not have the same amount of letters. "
"The current version of header_manipulation only allows for replacing a frame_id with a new of same length. "
"Otherwise receivers of the manipulted messages do report Buffer overflow.");
}
}
// for now, swap as to be done switch-wide, cannot be done on a per context
// basis, but this could easily be changed
RuntimeInterface::ErrorCode
SwitchWContexts::load_new_config(const std::string &new_config) {
if (!enable_swap) return ErrorCode::CONFIG_SWAP_DISABLED;
std::istringstream ss(new_config);
for (auto &cxt : contexts) {
ErrorCode rc = cxt.load_new_config(&ss, get_lookup_factory(),
required_fields, arith_objects);
if (rc != ErrorCode::SUCCESS) return rc;
ss.clear();
ss.seekg(0, std::ios::beg);
}
{
std::unique_lock<std::mutex> config_lock(config_mutex);
current_config = new_config;
}
return ErrorCode::SUCCESS;
}
void HeaderManipulation::manipulateRawData(uint8_t *const msg_buffer)
{
ROS_DEBUG("HeaderManipulation manipulateRawData");
std_msgs::Header header;;
header.seq = ((uint32_t *)msg_buffer)[0];
header.stamp.sec = ((uint32_t *)msg_buffer)[1];
header.stamp.nsec = ((uint32_t *)msg_buffer)[2];
// Extract frame_id.
uint frame_id_len = ((uint32_t *)msg_buffer)[3];
header.frame_id.resize(frame_id_len);
uint start_of_frame_id = 4*4;
for (uint i = 0; i < frame_id_len; ++i)
{
header.frame_id[i] = ((uint8_t *)msg_buffer)[start_of_frame_id+i];
}
// TODO (fkunz) check if header data is consistent
if ( fabs((header.stamp - ros::Time::now()).toSec()) > 5.0 )
{
ROS_WARN("Probably subscribed to a topic with a message type not "
"containing a header.Timestamp is: %f at Time: %f",
header.stamp.toSec(), ros::Time::now().toSec());
}
// Manipulate timestamp.
boost::mutex::scoped_lock config_lock(config_mutex_);
ROS_DEBUG("setting header stamp from %f to %f", header.stamp.toSec(),
(header.stamp + time_offset_).toSec());
header.stamp += time_offset_;
((uint32_t *)msg_buffer)[1] = header.stamp.sec;
((uint32_t *)msg_buffer)[2] = header.stamp.nsec;
// Manipulate frame_id.
if (frame_id_len == frame_id_to_replace_.length() &&
frame_id_to_replace_.length() == frame_id_new_.length())
{
for (uint i = 0; i < frame_id_new_.length(); ++i)
{
((uint8_t *)msg_buffer)[start_of_frame_id+i] = frame_id_new_.c_str()[i];
}
}
}
int
SwitchWContexts::init_objects(const std::string &json_path, int dev_id,
std::shared_ptr<TransportIface> transport) {
std::ifstream fs(json_path, std::ios::in);
if (!fs) {
std::cout << "JSON input file " << json_path << " cannot be opened\n";
return 1;
}
device_id = dev_id;
if (!transport) {
notifications_transport = std::shared_ptr<TransportIface>(
TransportIface::make_dummy());
} else {
notifications_transport = std::move(transport);
}
for (size_t cxt_id = 0; cxt_id < nb_cxts; cxt_id++) {
auto &cxt = contexts.at(cxt_id);
cxt.set_device_id(device_id);
cxt.set_notifications_transport(notifications_transport);
int status = cxt.init_objects(&fs, get_lookup_factory(),
required_fields, arith_objects);
fs.clear();
fs.seekg(0, std::ios::beg);
if (status != 0) return status;
phv_source->set_phv_factory(cxt_id, &cxt.get_phv_factory());
}
{
std::unique_lock<std::mutex> config_lock(config_mutex);
current_config = std::string((std::istreambuf_iterator<char>(fs)),
std::istreambuf_iterator<char>());
}
return 0;
}
void Stereoproc::imageCb(
const sensor_msgs::ImageConstPtr& l_raw_msg,
const sensor_msgs::CameraInfoConstPtr& l_info_msg,
const sensor_msgs::ImageConstPtr& r_raw_msg,
const sensor_msgs::CameraInfoConstPtr& r_info_msg)
{
boost::lock_guard<boost::recursive_mutex> config_lock(config_mutex_);
boost::lock_guard<boost::mutex> connect_lock(connect_mutex_);
int level = connected_.level();
NODELET_DEBUG("got images, level %d", level);
// Update the camera model
model_.fromCameraInfo(l_info_msg, r_info_msg);
cv::cuda::Stream l_strm, r_strm;
cv::cuda::GpuMat l_raw, r_raw;
std::vector<GPUSender::Ptr> senders;
// Create cv::Mat views onto all buffers
const cv::Mat l_cpu_raw = cv_bridge::toCvShare(
l_raw_msg,
l_raw_msg->encoding)->image;
cv::cuda::registerPageLocked(const_cast<cv::Mat&>(l_cpu_raw));
const cv::Mat r_cpu_raw = cv_bridge::toCvShare(
r_raw_msg,
l_raw_msg->encoding)->image;
cv::cuda::registerPageLocked(const_cast<cv::Mat&>(r_cpu_raw));
l_raw.upload(l_cpu_raw, l_strm);
r_raw.upload(r_cpu_raw, r_strm);
cv::cuda::GpuMat l_mono;
if(connected_.DebayerMonoLeft || connected_.RectifyMonoLeft || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
{
cv::cuda::demosaicing(l_raw, l_mono, CV_BayerRG2GRAY, 1, l_strm);
}
if(connected_.DebayerMonoLeft)
{
GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_left_));
senders.push_back(t);
t->enqueueSend(l_mono, l_strm);
}
cv::cuda::GpuMat r_mono;
if(connected_.DebayerMonoRight || connected_.RectifyMonoRight || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
{
cv::cuda::demosaicing(r_raw, r_mono, CV_BayerRG2GRAY, 1, r_strm);
}
if(connected_.DebayerMonoRight)
{
GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_right_));
senders.push_back(t);
t->enqueueSend(r_mono, r_strm);
}
cv::cuda::GpuMat l_color;
if(connected_.DebayerColorLeft || connected_.RectifyColorLeft || connected_.Pointcloud)
{
cv::cuda::demosaicing(l_raw, l_color, CV_BayerRG2BGR, 3, l_strm);
}
if(connected_.DebayerColorLeft)
{
GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_left_));
senders.push_back(t);
t->enqueueSend(l_color, l_strm);
}
cv::cuda::GpuMat r_color;
if(connected_.DebayerColorRight || connected_.RectifyColorRight)
{
cv::cuda::demosaicing(r_raw, r_color, CV_BayerRG2BGR, 3, r_strm);
}
if(connected_.DebayerColorRight)
{
GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_right_));
senders.push_back(t);
t->enqueueSend(r_color, r_strm);
}
cv::cuda::GpuMat l_rect_mono, r_rect_mono;
if(connected_.RectifyMonoLeft || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
{
model_.left().rectifyImageGPU(l_mono, l_rect_mono, cv::INTER_LINEAR, l_strm);
}
if(connected_.RectifyMonoRight || connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
{
model_.right().rectifyImageGPU(r_mono, r_rect_mono, cv::INTER_LINEAR, r_strm);
}
if(connected_.RectifyMonoLeft)
{
GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_rect_left_));
senders.push_back(t);
t->enqueueSend(l_rect_mono, l_strm);
}
if(connected_.RectifyMonoRight)
{
GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::MONO8, &pub_mono_rect_right_));
senders.push_back(t);
t->enqueueSend(r_rect_mono, r_strm);
}
cv::cuda::GpuMat l_rect_color, r_rect_color;
if(connected_.RectifyColorLeft || connected_.Pointcloud)
{
model_.left().rectifyImageGPU(l_color, l_rect_color, cv::INTER_LINEAR, l_strm);
}
if(connected_.RectifyColorRight)
{
model_.right().rectifyImageGPU(r_color, r_rect_color, cv::INTER_LINEAR, r_strm);
}
if(connected_.RectifyColorLeft)
{
GPUSender::Ptr t(new GPUSender(l_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_rect_left_));
senders.push_back(t);
t->enqueueSend(l_rect_color, l_strm);
}
if(connected_.RectifyColorRight)
{
GPUSender::Ptr t(new GPUSender(r_raw_msg, sensor_msgs::image_encodings::BGR8, &pub_color_rect_right_));
senders.push_back(t);
t->enqueueSend(r_rect_color, r_strm);
}
cv::cuda::GpuMat disparity;
cv::cuda::GpuMat disparity_16s;
cv::cuda::GpuMat disparity_filtered;
if(connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
{
r_strm.waitForCompletion();
block_matcher_->compute(l_rect_mono, r_rect_mono, disparity, l_strm);
//allocate cpu-side resource
filter_buf_.create(l_rect_mono.size(), CV_16SC1);
//enqueueDownload
disparity.convertTo(disparity_16s, CV_16SC1, 16, l_strm);
disparity_16s.download(filter_buf_, l_strm);
l_strm.waitForCompletion();
filterSpeckles();
//enqueueUpload
disparity_16s.upload(filter_buf_, l_strm);
if(bilateral_filter_enabled_)
{
bilateral_filter_->apply(disparity_16s, l_rect_mono, disparity_filtered);
disparity_filtered.convertTo(disparity, CV_32FC1, 1/16.);
} else {
disparity_16s.convertTo(disparity, CV_32FC1, 1/16.);
}
}
if(connected_.Disparity)
{
cv::cuda::GpuMat disparity_float;
if(disparity.type() != CV_32F)
disparity.convertTo(disparity_float, CV_32FC1);
else
disparity_float = disparity;
// Adjust for any x-offset between the principal points: d' = d - (cx_l - cx_r)
double cx_l = model_.left().cx();
double cx_r = model_.right().cx();
if (cx_l != cx_r)
{
cv::cuda::subtract(disparity_float, cv::Scalar(cx_l - cx_r),
disparity_float, cv::cuda::GpuMat(), -1, l_strm);
}
// Allocate new disparity image message
disp_msg_.reset(new stereo_msgs::DisparityImage());
disp_msg_->header = l_info_msg->header;
disp_msg_->image.header = l_info_msg->header;
// Compute window of (potentially) valid disparities
int border = block_matcher_->getBlockSize() / 2;
int left = block_matcher_->getNumDisparities() + block_matcher_min_disparity_ + border - 1;
int wtf = (block_matcher_min_disparity_ >= 0) ? border + block_matcher_min_disparity_ : std::max(border, -block_matcher_min_disparity_);
int right = disp_msg_->image.width - 1 - wtf;
int top = border;
int bottom = disp_msg_->image.height - 1 - border;
disp_msg_->valid_window.x_offset = left;
disp_msg_->valid_window.y_offset = top;
disp_msg_->valid_window.width = right - left;
disp_msg_->valid_window.height = bottom - top;
disp_msg_->min_disparity = block_matcher_min_disparity_ + 1;
disp_msg_->max_disparity = block_matcher_min_disparity_ + block_matcher_->getNumDisparities() - 1;
disp_msg_->image.height = l_rect_mono.rows;
disp_msg_->image.width = l_rect_mono.cols;
disp_msg_->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
disp_msg_->image.step = l_rect_mono.cols * sizeof(float);
disp_msg_->image.data.resize(disp_msg_->image.step*disp_msg_->image.height);
disp_msg_data_ = cv::Mat_<float> (disp_msg_->image.height,
disp_msg_->image.width,
(float *)&disp_msg_->image.data[0],
disp_msg_->image.step);
cv::cuda::registerPageLocked(disp_msg_data_);
disparity_float.download(disp_msg_data_, l_strm);
l_strm.enqueueHostCallback(
[](int status, void *userData)
{
(void)status;
static_cast<Stereoproc*>(userData)->sendDisparity();
},
(void*)this);
}
if(connected_.DisparityVis)
{
GPUSender::Ptr t(new GPUSender(l_raw_msg,
sensor_msgs::image_encodings::BGRA8, &pub_disparity_vis_));
senders.push_back(t);
cv::cuda::GpuMat disparity_int(l_cpu_raw.size(), CV_16SC1);
cv::cuda::GpuMat disparity_image(l_cpu_raw.size(), CV_8UC4);
int ndisp = block_matcher_->getNumDisparities();
if(disparity.type() == CV_32F)
{
disparity.convertTo(disparity_int, CV_16SC1, 16, 0, l_strm);
ndisp *= 16;
}
else
disparity_int = disparity;
try
{
cv::cuda::drawColorDisp(disparity_int, disparity_image, ndisp, l_strm);
t->enqueueSend(disparity_image, l_strm);
}
catch(cv::Exception e)
{
NODELET_ERROR_STREAM("Unable to draw color disparity: " << e.err <<
"in " << e.file << ":" << e.func <<
" line " << e.line);
}
}
cv::cuda::GpuMat xyz;
if(connected_.Pointcloud)
{
model_.projectDisparityImageTo3dGPU(disparity, xyz, true, l_strm);
cv::cuda::HostMem cuda_xyz(l_cpu_raw.size(), CV_32FC3);
xyz.download(cuda_xyz, l_strm);
cv::Mat l_cpu_color(l_cpu_raw.size(), CV_8UC3);
l_color.download(l_cpu_color, l_strm);
sensor_msgs::PointCloud2Ptr points_msg = boost::make_shared<sensor_msgs::PointCloud2>();
points_msg->header = l_raw_msg->header;
points_msg->height = l_cpu_raw.rows;
points_msg->width = l_cpu_raw.cols;
points_msg->fields.resize (4);
points_msg->fields[0].name = "x";
points_msg->fields[0].offset = 0;
points_msg->fields[0].count = 1;
points_msg->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
points_msg->fields[1].name = "y";
points_msg->fields[1].offset = 4;
points_msg->fields[1].count = 1;
points_msg->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
points_msg->fields[2].name = "z";
points_msg->fields[2].offset = 8;
points_msg->fields[2].count = 1;
points_msg->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
points_msg->fields[3].name = "rgb";
points_msg->fields[3].offset = 12;
points_msg->fields[3].count = 1;
points_msg->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
//points_msg->is_bigendian = false; ???
static const int STEP = 16;
points_msg->point_step = STEP;
points_msg->row_step = points_msg->point_step * points_msg->width;
points_msg->data.resize (points_msg->row_step * points_msg->height);
points_msg->is_dense = false; // there may be invalid points
l_strm.waitForCompletion();
cv::Mat_<cv::Vec3f> cpu_xyz = cuda_xyz.createMatHeader();
float bad_point = std::numeric_limits<float>::quiet_NaN ();
int offset = 0;
for (int v = 0; v < cpu_xyz.rows; ++v)
{
for (int u = 0; u < cpu_xyz.cols; ++u, offset += STEP)
{
if (isValidPoint(cpu_xyz(v,u)))
{
// x,y,z,rgba
memcpy (&points_msg->data[offset + 0], &cpu_xyz(v,u)[0], sizeof (float));
memcpy (&points_msg->data[offset + 4], &cpu_xyz(v,u)[1], sizeof (float));
memcpy (&points_msg->data[offset + 8], &cpu_xyz(v,u)[2], sizeof (float));
}
else
{
memcpy (&points_msg->data[offset + 0], &bad_point, sizeof (float));
memcpy (&points_msg->data[offset + 4], &bad_point, sizeof (float));
memcpy (&points_msg->data[offset + 8], &bad_point, sizeof (float));
}
}
}
// Fill in color
offset = 0;
const cv::Mat_<cv::Vec3b> color(l_cpu_color);
for (int v = 0; v < cpu_xyz.rows; ++v)
{
for (int u = 0; u < cpu_xyz.cols; ++u, offset += STEP)
{
if (isValidPoint(cpu_xyz(v,u)))
{
const cv::Vec3b& bgr = color(v,u);
int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
memcpy (&points_msg->data[offset + 12], &rgb_packed, sizeof (int32_t));
}
else
{
memcpy (&points_msg->data[offset + 12], &bad_point, sizeof (float));
}
}
}
pub_pointcloud_.publish(points_msg);
}
l_strm.waitForCompletion();
r_strm.waitForCompletion();
if(connected_.Disparity)
cv::cuda::unregisterPageLocked(disp_msg_data_);
//if(connected_.Disparity || connected_.DisparityVis || connected_.Pointcloud)
// cv::cuda::unregisterPageLocked(filter_buf_);
cv::cuda::unregisterPageLocked( const_cast<cv::Mat&>(l_cpu_raw) );
cv::cuda::unregisterPageLocked( const_cast<cv::Mat&>(r_cpu_raw) );
}
std::string
SwitchWContexts::get_config_md5() const {
std::unique_lock<std::mutex> config_lock(config_mutex);
return get_config_md5_();
}
std::string
SwitchWContexts::get_config() const {
std::unique_lock<std::mutex> config_lock(config_mutex);
return current_config;
}
void HeaderManipulation::publishMsg(const topic_tools::ShapeShifter &msg, const ros::Time &msg_in_time)
{
ROS_DEBUG("HeaderManipulation publishMsg Thread started with timestamp %f", msg_in_time.toSec());
ros::Time end_time(ros::Time::now());
ros::Time last_time;
config_mutex_.lock();
ros::Time time_to_pub(msg_in_time + msg_delay_);
config_mutex_.unlock();
do
{
last_time = end_time;
ROS_DEBUG("waiting to publish msg from %f at %f", msg_in_time.toSec(), time_to_pub.toSec());
if (end_time > time_to_pub)
{
boost::mutex::scoped_lock pub_lock(pub_mutex_);
ROS_DEBUG("publishing msg which should be held back till: %f", time_to_pub.toSec());
generic_pub_.publish(msg);
return;
}
boost::mutex::scoped_lock config_lock(config_mutex_);
publish_retry_rate_.sleep();
time_to_pub = msg_in_time + msg_delay_;
end_time = ros::Time::now();
} while (last_time <= end_time);
ROS_WARN("Detected jump back in time. Dropping msg. last: %f, end %f",
last_time.toSec(), end_time.toSec());
return;
}
