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;
}
Beispiel #2
0
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);
}
Beispiel #3
0
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);
}
Beispiel #4
0
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;
}
Beispiel #6
0
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;
}
Beispiel #8
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;
}
Beispiel #11
-1
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;
}