<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/planner/open_space/open_space_planner.h"

#include <fstream>
#include <utility>

#include "cyber/common/log.h"
#include "cyber/task/task.h"
#include "modules/common/util/string_tokenizer.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;

Status OpenSpacePlanner::Init(const PlanningConfig& planning_confgs) {
  AINFO << "In OpenSpacePlanner::Init()";

  // TODO(QiL): integrate open_space planner into task config when refactor done
  CHECK(common::util::GetProtoFromFile(FLAGS_planner_open_space_config_filename,
                                       &planner_open_space_config_))
      << "Failed to load open space config file "
      << FLAGS_planner_open_space_config_filename;

  current_trajectory_index_ = 0;
  // initialize open space trajectory generator
  open_space_trajectory_generator_.reset(new OpenSpaceTrajectoryGenerator());

  open_space_trajectory_generator_->Init(planner_open_space_config_);

  // initialize open space region of interest generator
  if (FLAGS_enable_open_space_roi_and_info) {
    open_space_roi_generator_.reset(new OpenSpaceROI());
  }

  if (FLAGS_enable_open_space_planner_thread) {
    task_future_ =
        cyber::Async(&OpenSpacePlanner::GenerateTrajectoryThread, this);
  }

  return Status::OK();
}

apollo::common::Status OpenSpacePlanner::Plan(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // 1. Build Predicition Environments.
  predicted_bounding_rectangles_.clear();
  if (FLAGS_enable_open_space_planner_thread) {
    std::lock_guard<std::mutex> lock(open_space_mutex_);
    BuildPredictedEnvironment(frame->obstacles());

    // 2. Update Vehicle information and obstacles information from frame.

    vehicle_state_ = frame->vehicle_state();
    if (!open_space_roi_generator_->GenerateRegionOfInterest(frame)) {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Generate Open Space ROI failed");
    }
    rotate_angle_ = open_space_roi_generator_->origin_heading();
    translate_origin_ = open_space_roi_generator_->origin_point();
    end_pose_ = open_space_roi_generator_->open_space_end_pose();
    obstacles_num_ = open_space_roi_generator_->obstacles_num();
    obstacles_edges_num_ = open_space_roi_generator_->obstacles_edges_num();
    obstacles_A_ = open_space_roi_generator_->obstacles_A();
    obstacles_b_ = open_space_roi_generator_->obstacles_b();
    obstalce_list_ = open_space_roi_generator_->openspace_warmstart_obstacles();
    XYbounds_ = open_space_roi_generator_->ROI_xy_boundary();
    // 3. Check if trajectory updated, if so, update internal
    // trajectory_partition_;
    if (trajectory_updated_) {
      trajectory_partition_.Clear();
      gear_positions_.clear();
      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                         &gear_positions_);
      AINFO << "Trajectory caculation updated, new results : "
            << trajectory_partition_.ShortDebugString();
    }
    // TODO(Jiaxuan): Choose the current_trajectory in trajectory_partition_
    // If the vehicle each the end point of current trajectory and stop
    // Then move to the next Trajectory
    current_trajectory_ =
        trajectory_partition_.trajectory(current_trajectory_index_);

    TrajectoryPoint end_point = current_trajectory_.trajectory_point(
        current_trajectory_.trajectory_point_size() - 1);

    if (vehicle_state_.linear_velocity() <= 1e-3 &&
        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                      (vehicle_state_.x() - end_point.path_point().x()) +
                  (vehicle_state_.y() - end_point.path_point().y()) *
                      (vehicle_state_.y() - end_point.path_point().y())) <
            planner_open_space_config_.max_position_error_to_end_point() &&
        std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
            planner_open_space_config_.max_theta_error_to_end_point() &&
        (current_trajectory_index_ <
         trajectory_partition_.trajectory_size() - 1)) {
      current_trajectory_index_ += 1;
    }

    // 4. Collision check for updated trajectory, if pass, update frame, else,
    // return error status

    if (IsCollisionFreeTrajectory(current_trajectory_)) {
      // Convet current trajectory to publishable_trajectory
      //  frame->mutable_trajectory()->CopyFrom(current_trajectory_);
      return Status::OK();
    } else {
      // If collision happens, return wrong planning status and estop
      // trajectory would be sent in std planning
      return Status(ErrorCode::PLANNING_ERROR, "Collision Check failed");
    }
  } else {
    // Single thread logic

    vehicle_state_ = frame->vehicle_state();
    if (!open_space_roi_generator_->GenerateRegionOfInterest(frame)) {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Generate Open Space ROI failed");
    }
    rotate_angle_ = open_space_roi_generator_->origin_heading();
    translate_origin_ = open_space_roi_generator_->origin_point();
    end_pose_ = open_space_roi_generator_->open_space_end_pose();
    obstacles_num_ = open_space_roi_generator_->obstacles_num();
    obstacles_edges_num_ = open_space_roi_generator_->obstacles_edges_num();
    obstacles_A_ = open_space_roi_generator_->obstacles_A();
    obstacles_b_ = open_space_roi_generator_->obstacles_b();
    obstalce_list_ = open_space_roi_generator_->openspace_warmstart_obstacles();
    XYbounds_ = open_space_roi_generator_->ROI_xy_boundary();

    // 2. Generate Trajectory;
    Status status = open_space_trajectory_generator_->Plan(
        vehicle_state_, XYbounds_, rotate_angle_, translate_origin_, end_pose_,
        obstacles_num_, obstacles_edges_num_, obstacles_A_, obstacles_b_,
        obstalce_list_);

    // 3. If status is OK, update vehicle trajectory;
    if (status == Status::OK()) {
      trajectory_partition_.Clear();
      gear_positions_.clear();
      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                         &gear_positions_);
      AINFO << "Trajectory caculation updated, new results : "
            << trajectory_partition_.ShortDebugString();
    } else {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Planning failed to generate open space trajectory");
    }

    current_trajectory_ =
        trajectory_partition_.trajectory(current_trajectory_index_);

    TrajectoryPoint end_point = current_trajectory_.trajectory_point(
        current_trajectory_.trajectory_point_size() - 1);

    if (vehicle_state_.linear_velocity() <= 1e-3 &&
        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                      (vehicle_state_.x() - end_point.path_point().x()) +
                  (vehicle_state_.y() - end_point.path_point().y()) *
                      (vehicle_state_.y() - end_point.path_point().y())) <
            planner_open_space_config_.max_position_error_to_end_point() &&
        std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
            planner_open_space_config_.max_theta_error_to_end_point() &&
        (current_trajectory_index_ <
         trajectory_partition_.trajectory_size() - 1)) {
      current_trajectory_index_ += 1;
    }

    // 4. Collision check for updated trajectory, if pass, update frame, else,
    // return error status

    if (IsCollisionFreeTrajectory(current_trajectory_)) {
      publishable_trajectory_.Clear();
      publishable_trajectory_.MergeFrom(current_trajectory_);
      frame->mutable_trajectory()->CopyFrom(publishable_trajectory_);
      return Status::OK();
    } else {
      // If collision happens, return wrong planning status and estop
      // trajectory would be sent in std planning
      return Status(ErrorCode::PLANNING_ERROR, "Collision Check failed");
    }
  }
}

void OpenSpacePlanner::GenerateTrajectoryThread() {
  while (!is_stop_) {
    {
      std::lock_guard<std::mutex> lock(open_space_mutex_);
      trajectory_updated_ = false;
      if (open_space_trajectory_generator_->Plan(
              vehicle_state_, XYbounds_, rotate_angle_, translate_origin_,
              end_pose_, obstacles_num_, obstacles_edges_num_, obstacles_A_,
              obstacles_b_, obstalce_list_) == Status::OK()) {
        trajectory_updated_ = true;
      }
    }
  }
}

void OpenSpacePlanner::Stop() {
  is_stop_ = true;
  if (FLAGS_enable_open_space_planner_thread) {
    task_future_.get();
  }
}

bool OpenSpacePlanner::IsCollisionFreeTrajectory(
    const apollo::common::Trajectory& trajectory) {
  CHECK_LE(trajectory.trajectory_point().size(),
           predicted_bounding_rectangles_.size());
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();

  for (int i = 0; i < trajectory.trajectory_point().size(); ++i) {
    const auto& trajectory_point = trajectory.trajectory_point(i);
    double ego_theta = trajectory_point.path_point().theta();
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);

    for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return false;
      }
    }
  }
  return true;
}

void OpenSpacePlanner::BuildPredictedEnvironment(
    const std::vector<const Obstacle*>& obstacles) {
  CHECK(predicted_bounding_rectangles_.empty());
  double relative_time = 0.0;
  while (relative_time < FLAGS_trajectory_time_length) {
    std::vector<Box2d> predicted_env;
    for (const Obstacle* obstacle : obstacles) {
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      Box2d box = obstacle->GetBoundingBox(point);
      predicted_env.push_back(std::move(box));
    }
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning : fix the trajectory update in open space planner in multi-threads<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/planner/open_space/open_space_planner.h"

#include <fstream>
#include <utility>

#include "cyber/common/log.h"
#include "cyber/task/task.h"
#include "modules/common/util/string_tokenizer.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Box2d;
using apollo::common::math::Vec2d;

Status OpenSpacePlanner::Init(const PlanningConfig& planning_confgs) {
  AINFO << "In OpenSpacePlanner::Init()";

  // TODO(QiL): integrate open_space planner into task config when refactor done
  CHECK(common::util::GetProtoFromFile(FLAGS_planner_open_space_config_filename,
                                       &planner_open_space_config_))
      << "Failed to load open space config file "
      << FLAGS_planner_open_space_config_filename;

  current_trajectory_index_ = 0;
  // initialize open space trajectory generator
  open_space_trajectory_generator_.reset(new OpenSpaceTrajectoryGenerator());

  open_space_trajectory_generator_->Init(planner_open_space_config_);

  // initialize open space region of interest generator
  if (FLAGS_enable_open_space_roi_and_info) {
    open_space_roi_generator_.reset(new OpenSpaceROI());
  }

  if (FLAGS_enable_open_space_planner_thread) {
    task_future_ =
        cyber::Async(&OpenSpacePlanner::GenerateTrajectoryThread, this);
  }

  return Status::OK();
}

apollo::common::Status OpenSpacePlanner::Plan(
    const common::TrajectoryPoint& planning_init_point, Frame* frame) {
  // 1. Build Predicition Environments.
  predicted_bounding_rectangles_.clear();
  if (FLAGS_enable_open_space_planner_thread) {
    std::lock_guard<std::mutex> lock(open_space_mutex_);
    BuildPredictedEnvironment(frame->obstacles());

    // 2. Update Vehicle information and obstacles information from frame.

    vehicle_state_ = frame->vehicle_state();
    if (!open_space_roi_generator_->GenerateRegionOfInterest(frame)) {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Generate Open Space ROI failed");
    }
    rotate_angle_ = open_space_roi_generator_->origin_heading();
    translate_origin_ = open_space_roi_generator_->origin_point();
    end_pose_ = open_space_roi_generator_->open_space_end_pose();
    obstacles_num_ = open_space_roi_generator_->obstacles_num();
    obstacles_edges_num_ = open_space_roi_generator_->obstacles_edges_num();
    obstacles_A_ = open_space_roi_generator_->obstacles_A();
    obstacles_b_ = open_space_roi_generator_->obstacles_b();
    obstalce_list_ = open_space_roi_generator_->openspace_warmstart_obstacles();
    XYbounds_ = open_space_roi_generator_->ROI_xy_boundary();
    // 3. Check if trajectory updated, if so, update internal
    // trajectory_partition_;
    if (trajectory_updated_) {
      trajectory_partition_.Clear();
      gear_positions_.clear();
      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                         &gear_positions_);
      AINFO << "Trajectory caculation updated, new results : "
            << trajectory_partition_.ShortDebugString();
    }
    // TODO(Jiaxuan): Choose the current_trajectory in trajectory_partition_
    // If the vehicle each the end point of current trajectory and stop
    // Then move to the next Trajectory
    current_trajectory_ =
        trajectory_partition_.trajectory(current_trajectory_index_);

    TrajectoryPoint end_point = current_trajectory_.trajectory_point(
        current_trajectory_.trajectory_point_size() - 1);

    if (vehicle_state_.linear_velocity() <= 1e-3 &&
        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                      (vehicle_state_.x() - end_point.path_point().x()) +
                  (vehicle_state_.y() - end_point.path_point().y()) *
                      (vehicle_state_.y() - end_point.path_point().y())) <
            planner_open_space_config_.max_position_error_to_end_point() &&
        std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
            planner_open_space_config_.max_theta_error_to_end_point() &&
        (current_trajectory_index_ <
         trajectory_partition_.trajectory_size() - 1)) {
      current_trajectory_index_ += 1;
    }

    // 4. Collision check for updated trajectory, if pass, update frame, else,
    // return error status

    if (IsCollisionFreeTrajectory(current_trajectory_)) {
      // Convet current trajectory to publishable_trajectory
      publishable_trajectory_.Clear();
      publishable_trajectory_.MergeFrom(current_trajectory_);
      frame->mutable_trajectory()->CopyFrom(publishable_trajectory_);
      return Status::OK();
    } else {
      // If collision happens, return wrong planning status and estop
      // trajectory would be sent in std planning
      return Status(ErrorCode::PLANNING_ERROR, "Collision Check failed");
    }
  } else {
    // Single thread logic

    vehicle_state_ = frame->vehicle_state();
    if (!open_space_roi_generator_->GenerateRegionOfInterest(frame)) {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Generate Open Space ROI failed");
    }
    rotate_angle_ = open_space_roi_generator_->origin_heading();
    translate_origin_ = open_space_roi_generator_->origin_point();
    end_pose_ = open_space_roi_generator_->open_space_end_pose();
    obstacles_num_ = open_space_roi_generator_->obstacles_num();
    obstacles_edges_num_ = open_space_roi_generator_->obstacles_edges_num();
    obstacles_A_ = open_space_roi_generator_->obstacles_A();
    obstacles_b_ = open_space_roi_generator_->obstacles_b();
    obstalce_list_ = open_space_roi_generator_->openspace_warmstart_obstacles();
    XYbounds_ = open_space_roi_generator_->ROI_xy_boundary();

    // 2. Generate Trajectory;
    Status status = open_space_trajectory_generator_->Plan(
        vehicle_state_, XYbounds_, rotate_angle_, translate_origin_, end_pose_,
        obstacles_num_, obstacles_edges_num_, obstacles_A_, obstacles_b_,
        obstalce_list_);

    // 3. If status is OK, update vehicle trajectory;
    if (status == Status::OK()) {
      trajectory_partition_.Clear();
      gear_positions_.clear();
      open_space_trajectory_generator_->UpdateTrajectory(&trajectory_partition_,
                                                         &gear_positions_);
      AINFO << "Trajectory caculation updated, new results : "
            << trajectory_partition_.ShortDebugString();
    } else {
      return Status(ErrorCode::PLANNING_ERROR,
                    "Planning failed to generate open space trajectory");
    }

    current_trajectory_ =
        trajectory_partition_.trajectory(current_trajectory_index_);

    TrajectoryPoint end_point = current_trajectory_.trajectory_point(
        current_trajectory_.trajectory_point_size() - 1);

    if (vehicle_state_.linear_velocity() <= 1e-3 &&
        std::sqrt((vehicle_state_.x() - end_point.path_point().x()) *
                      (vehicle_state_.x() - end_point.path_point().x()) +
                  (vehicle_state_.y() - end_point.path_point().y()) *
                      (vehicle_state_.y() - end_point.path_point().y())) <
            planner_open_space_config_.max_position_error_to_end_point() &&
        std::abs(vehicle_state_.heading() - end_point.path_point().theta()) <
            planner_open_space_config_.max_theta_error_to_end_point() &&
        (current_trajectory_index_ <
         trajectory_partition_.trajectory_size() - 1)) {
      current_trajectory_index_ += 1;
    }

    // 4. Collision check for updated trajectory, if pass, update frame, else,
    // return error status

    if (IsCollisionFreeTrajectory(current_trajectory_)) {
      publishable_trajectory_.Clear();
      publishable_trajectory_.MergeFrom(current_trajectory_);
      frame->mutable_trajectory()->CopyFrom(publishable_trajectory_);
      return Status::OK();
    } else {
      // If collision happens, return wrong planning status and estop
      // trajectory would be sent in std planning
      return Status(ErrorCode::PLANNING_ERROR, "Collision Check failed");
    }
  }
}

void OpenSpacePlanner::GenerateTrajectoryThread() {
  while (!is_stop_) {
    {
      std::lock_guard<std::mutex> lock(open_space_mutex_);
      trajectory_updated_ = false;
      if (open_space_trajectory_generator_->Plan(
              vehicle_state_, XYbounds_, rotate_angle_, translate_origin_,
              end_pose_, obstacles_num_, obstacles_edges_num_, obstacles_A_,
              obstacles_b_, obstalce_list_) == Status::OK()) {
        trajectory_updated_ = true;
      }
    }
  }
}

void OpenSpacePlanner::Stop() {
  is_stop_ = true;
  if (FLAGS_enable_open_space_planner_thread) {
    task_future_.get();
  }
}

bool OpenSpacePlanner::IsCollisionFreeTrajectory(
    const apollo::common::Trajectory& trajectory) {
  CHECK_LE(trajectory.trajectory_point().size(),
           predicted_bounding_rectangles_.size());
  const auto& vehicle_config =
      common::VehicleConfigHelper::Instance()->GetConfig();
  double ego_length = vehicle_config.vehicle_param().length();
  double ego_width = vehicle_config.vehicle_param().width();

  for (int i = 0; i < trajectory.trajectory_point().size(); ++i) {
    const auto& trajectory_point = trajectory.trajectory_point(i);
    double ego_theta = trajectory_point.path_point().theta();
    Box2d ego_box(
        {trajectory_point.path_point().x(), trajectory_point.path_point().y()},
        ego_theta, ego_length, ego_width);
    double shift_distance =
        ego_length / 2.0 - vehicle_config.vehicle_param().back_edge_to_center();
    Vec2d shift_vec{shift_distance * std::cos(ego_theta),
                    shift_distance * std::sin(ego_theta)};
    ego_box.Shift(shift_vec);

    for (const auto& obstacle_box : predicted_bounding_rectangles_[i]) {
      if (ego_box.HasOverlap(obstacle_box)) {
        return false;
      }
    }
  }
  return true;
}

void OpenSpacePlanner::BuildPredictedEnvironment(
    const std::vector<const Obstacle*>& obstacles) {
  CHECK(predicted_bounding_rectangles_.empty());
  double relative_time = 0.0;
  while (relative_time < FLAGS_trajectory_time_length) {
    std::vector<Box2d> predicted_env;
    for (const Obstacle* obstacle : obstacles) {
      TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
      Box2d box = obstacle->GetBoundingBox(point);
      predicted_env.push_back(std::move(box));
    }
    predicted_bounding_rectangles_.push_back(std::move(predicted_env));
    relative_time += FLAGS_trajectory_time_resolution;
  }
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/*********************************************************************************
 *
 * Inviwo - Interactive Visualization Workshop
 *
 * Copyright (c) 2017-2019 Inviwo Foundation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************************/

#include <modules/plottinggl/processors/axisrenderprocessor.h>

#include <modules/plotting/utils/axisutils.h>
#include <modules/opengl/texture/textureutils.h>
#include <modules/opengl/rendering/texturequadrenderer.h>

namespace inviwo {

namespace plot {

// The Class Identifier has to be globally unique. Use a reverse DNS naming scheme
const ProcessorInfo AxisRenderProcessor::processorInfo_{
    "org.inviwo.AxisRenderProcessor",  // Class identifier
    "Axis Render Processor",           // Display name
    "Plotting",                        // Category
    CodeState::Stable,                 // Code state
    "GL, Plotting, Demo",              // Tags
};
const ProcessorInfo AxisRenderProcessor::getProcessorInfo() const { return processorInfo_; }

AxisRenderProcessor::AxisRenderProcessor()
    : Processor()
    , inport_("inport")
    , outport_("outport")
    , margins_("margins", "Margins", 5.0f, 5.0f, 55.0f, 60.0f)
    , axisMargin_("axisMargin", "Axis Margin", 15.0f, 0.0f, 50.0f)
    , antialiasing_("antialias", "Antialiasing", true)
    , style_("style", "Global Style")
    , axis1_("axis1", "Axis 1")
    , axis2_("axis2", "Axis 2", AxisProperty::Orientation::Vertical)
    , axis3_("axis3", "Axis 3")
    , axisRenderers_({axis1_, axis2_, axis3_}) {

    inport_.setOptional(true);

    addPort(inport_);
    addPort(outport_);

    addProperty(margins_);
    addProperty(axisMargin_);

    addProperty(antialiasing_);

    axis1_.captionSettings_.setChecked(true);
    axis1_.setCaption("x Axis");

    // flip vertical axis to show labels on the left side
    axis2_.flipped_.set(true);
    axis2_.captionSettings_.setChecked(true);
    axis2_.setCaption("y Axis");

    axis3_.captionSettings_.setChecked(true);
    axis3_.setCaption("Diagonal Axis");

    int var = 0;

    auto& a = axis3_;

    style_.setCollapsed(true);
    style_.registerProperties(axis1_, axis2_, axis3_);
    addProperties(style_, axis1_, axis2_, axis3_);
}

void AxisRenderProcessor::process() {
    utilgl::activateTargetAndClearOrCopySource(outport_, inport_, ImageType::ColorDepth);

    const auto dims = outport_.getDimensions();
    const size2_t lowerLeft(margins_.getLeft(), margins_.getBottom());
    const size2_t upperRight(dims.x - 1 - margins_.getRight(), dims.y - 1 - margins_.getTop());

    const auto padding = axisMargin_.get();

    // draw horizontally
    axisRenderers_[0].render(dims, lowerLeft + size2_t(padding, 0),
                             size2_t(upperRight.x - padding, lowerLeft.y), antialiasing_.get());

    // draw vertically
    axisRenderers_[1].render(dims, lowerLeft + size2_t(0, padding),
                             size2_t(lowerLeft.x, upperRight.y - padding), antialiasing_.get());

    // draw diagonally in upper half
    axisRenderers_[2].render(
        dims, size2_t(lowerLeft.x + padding, (lowerLeft.y + upperRight.y) * 0.5f),
        size2_t(upperRight.x - padding, upperRight.y - padding), antialiasing_.get());

    utilgl::deactivateCurrentTarget();
}

}  // namespace plot

}  // namespace inviwo
<commit_msg>Jenkins: test<commit_after>/*********************************************************************************
 *
 * Inviwo - Interactive Visualization Workshop
 *
 * Copyright (c) 2017-2019 Inviwo Foundation
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************************/

#include <modules/plottinggl/processors/axisrenderprocessor.h>

#include <modules/plotting/utils/axisutils.h>
#include <modules/opengl/texture/textureutils.h>
#include <modules/opengl/rendering/texturequadrenderer.h>

namespace inviwo {

namespace plot {

// The Class Identifier has to be globally unique. Use a reverse DNS naming scheme
const ProcessorInfo AxisRenderProcessor::processorInfo_{
    "org.inviwo.AxisRenderProcessor",  // Class identifier
    "Axis Render Processor",           // Display name
    "Plotting",                        // Category
    CodeState::Stable,                 // Code state
    "GL, Plotting, Demo",              // Tags
};
const ProcessorInfo AxisRenderProcessor::getProcessorInfo() const { return processorInfo_; }

AxisRenderProcessor::AxisRenderProcessor()
    : Processor()
    , inport_("inport")
    , outport_("outport")
    , margins_("margins", "Margins", 5.0f, 5.0f, 55.0f, 60.0f)
    , axisMargin_("axisMargin", "Axis Margin", 15.0f, 0.0f, 50.0f)
    , antialiasing_("antialias", "Antialiasing", true)
    , style_("style", "Global Style")
    , axis1_("axis1", "Axis 1")
    , axis2_("axis2", "Axis 2", AxisProperty::Orientation::Vertical)
    , axis3_("axis3", "Axis 3")
    , axisRenderers_({axis1_, axis2_, axis3_}) {

    inport_.setOptional(true);

    addPort(inport_);
    addPort(outport_);

    addProperty(margins_);
    addProperty(axisMargin_);

    addProperty(antialiasing_);

    axis1_.captionSettings_.setChecked(true);
    axis1_.setCaption("x Axis");

    // flip vertical axis to show labels on the left side
    axis2_.flipped_.set(true);
    axis2_.captionSettings_.setChecked(true);
    axis2_.setCaption("y Axis");

    axis3_.captionSettings_.setChecked(true);
    axis3_.setCaption("Diagonal Axis");

    int var = 0;

    auto& a = axis3_;

        style_.setCollapsed(true);
                    style_.registerProperties(axis1_, axis2_, axis3_);
    addProperties(style_, axis1_, axis2_, axis3_);
}

void AxisRenderProcessor::process() {
    utilgl::activateTargetAndClearOrCopySource(outport_, inport_, ImageType::ColorDepth);

    const auto dims = outport_.getDimensions();
    const size2_t lowerLeft(margins_.getLeft(), margins_.getBottom());
    const size2_t upperRight(dims.x - 1 - margins_.getRight(), dims.y - 1 - margins_.getTop());

    const auto padding = axisMargin_.get();

    // draw horizontally
    axisRenderers_[0].render(dims, lowerLeft + size2_t(padding, 0),
                             size2_t(upperRight.x - padding, lowerLeft.y), antialiasing_.get());

    // draw vertically
    axisRenderers_[1].render(dims, lowerLeft + size2_t(0, padding),
                             size2_t(lowerLeft.x, upperRight.y - padding), antialiasing_.get());

    // draw diagonally in upper half
    axisRenderers_[2].render(
        dims, size2_t(lowerLeft.x + padding, (lowerLeft.y + upperRight.y) * 0.5f),
        size2_t(upperRight.x - padding, upperRight.y - padding), antialiasing_.get());

    utilgl::deactivateCurrentTarget();
}

}  // namespace plot

}  // namespace inviwo
<|endoftext|>


<commit_before>/* ports.cpp
 *
 * Copyright (c) 2011, 2012 Chantilly Robotics <chantilly612@gmail.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * This file contains tables for the electrical layout and setup of the
 * electrical components.  Keep the formatting in this file very easy to read
 * such that it is easy to maintain and quickly check if it is in sync with
 * the actual state of affairs on the electrical board!
 */

#include <RobotDrive.h>
#include <Jaguar.h>
#include <Joystick.h>
#include <Encoder.h>
#include <Ultrasonic.h>
#include <DigitalInput.h>
#include <Relay.h>
#include <Servo.h>
#include <AnalogChannel.h>
#include <Vision/AxisCamera.h>
#include "612.h"
#include "ports.h"
#include "joysmooth.h"

//just define & initialize all of the consts in ports.h

//slots table - in order to make sync with electrical easier.
const module_t slot1 = 1; //1st analog
const module_t slot2 = 1; //1st digital
const module_t slot3 = 1; //1st solenoid
const module_t slot4 = 0; //EMPTY
const module_t slot5 = 2; //2nd analog
const module_t slot6 = 2; //2nd digital
const module_t slot7 = 2; //2nd solenoid
const module_t slot8 = 0; //EMPTY

//PORTS TABLE

//PWMs                                    SLOT     PORT
Jaguar turret_XY_control_jag            ( slot2,     1 );
Jaguar turret_Z_control_jag             ( slot2,     2 );
Jaguar right_front_jag                  ( slot2,     3 );
Jaguar right_rear_jag                   ( slot2,     4 );
Servo right_servo_shift                 ( slot2,     5 );

Jaguar left_front_jag                   ( slot6,     1 );
Jaguar left_rear_jag                    ( slot6,     2 );
Jaguar right_launcher_jag               ( slot6,     3 );
Jaguar left_launcher_jag                ( slot6,     4 );
Servo left_servo_shift                  ( slot6,     5 );

//NOTE: Sica wants to use pots (potentiometers) for the angles, as those are
//absolute and don't need a zero switch.  If we do that they're seen as
//AnalogChannel s.  We'll want to write a quick wrapper class so we can get
//an angular offset, but we'll cross that bridge when we come to it.

//note: for two channel encoders, we need to specify a slot for both ports
//DIOs                                    SLOT     PORT
Encoder right_drive                     ( slot2,     1,
                                          slot2,     2 );
Encoder left_drive                      ( slot2,     3,
                                          slot2,     4 );
Encoder launcher_wheel                  ( slot2,     5,
                                          slot2,     6 );
Encoder launch_angle                    ( slot2,     7,
                                          slot2,     8 );

DigitalInput launch_angle_switch        ( slot6,     1 );
Ultrasonic front_ultrasonic             ( slot6,     2,
                                          slot6,     3 );

//note: since we rely on the default value of kInches for the 5th arg
//we should use Ultrasonic::GetRangeInches().

//AIOs                                    SLOT     PORT
AnalogChannel launch_angle_pot          ( slot1,     1 );

//Relays                                  SLOT     PORT
Relay roller_spike_1                    ( slot6,     1 );
Relay roller_spike_2                    ( slot6,     2 );
Relay bridge_arm_spike                  ( slot6,     3 );

//USBs (on driver station)                         PORT
Joystick left_joystick_raw                         ( 1 );
Joystick right_joystick_raw                        ( 2 );
Joystick gunner_joystick_raw                       ( 3 );

joysmooth left_joystick        (  left_joystick_raw    );
joysmooth right_joystick       (  right_joystick_raw   );
joysmooth gunner_joystick      (  gunner_joystick_raw  );

//initialization of virtual devices:

//RobotDrive
RobotDrive drive (
    left_front_motor.jag,  //KEEP THIS ORDER (HERE)!  IT'S IMPORTANT!
    left_rear_motor.jag,   //LF, LR, RF, RR -- see documentation
    right_front_motor.jag, //for more details
    right_rear_motor.jag
);

//drive_jaguar                           JAGUAR&                 TYPE               REVERSE
drive_jaguar left_front_motor  =    { left_front_jag,  RobotDrive::kFrontLeftMotor,  true };
drive_jaguar left_rear_motor   =    { left_rear_jag,   RobotDrive::kRearLeftMotor,   true };
drive_jaguar right_front_motor =    { right_front_jag, RobotDrive::kFrontRightMotor, true };
drive_jaguar right_rear_motor  =    { right_rear_jag,  RobotDrive::kRearRightMotor,  true };
<commit_msg>Removed launch angle encoder<commit_after>/* ports.cpp
 *
 * Copyright (c) 2011, 2012 Chantilly Robotics <chantilly612@gmail.com>
 *
 * Permission to use, copy, modify, and/or distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/*
 * This file contains tables for the electrical layout and setup of the
 * electrical components.  Keep the formatting in this file very easy to read
 * such that it is easy to maintain and quickly check if it is in sync with
 * the actual state of affairs on the electrical board!
 */

#include <RobotDrive.h>
#include <Jaguar.h>
#include <Joystick.h>
#include <Encoder.h>
#include <Ultrasonic.h>
#include <DigitalInput.h>
#include <Relay.h>
#include <Servo.h>
#include <AnalogChannel.h>
#include <Vision/AxisCamera.h>
#include "612.h"
#include "ports.h"
#include "joysmooth.h"

//just define & initialize all of the consts in ports.h

//slots table - in order to make sync with electrical easier.
const module_t slot1 = 1; //1st analog
const module_t slot2 = 1; //1st digital
const module_t slot3 = 1; //1st solenoid
const module_t slot4 = 0; //EMPTY
const module_t slot5 = 2; //2nd analog
const module_t slot6 = 2; //2nd digital
const module_t slot7 = 2; //2nd solenoid
const module_t slot8 = 0; //EMPTY

//PORTS TABLE

//PWMs                                    SLOT     PORT
Jaguar turret_XY_control_jag            ( slot2,     1 );
Jaguar turret_Z_control_jag             ( slot2,     2 );
Jaguar right_front_jag                  ( slot2,     3 );
Jaguar right_rear_jag                   ( slot2,     4 );
Servo right_servo_shift                 ( slot2,     5 );

Jaguar left_front_jag                   ( slot6,     1 );
Jaguar left_rear_jag                    ( slot6,     2 );
Jaguar right_launcher_jag               ( slot6,     3 );
Jaguar left_launcher_jag                ( slot6,     4 );
Servo left_servo_shift                  ( slot6,     5 );

//NOTE: Sica wants to use pots (potentiometers) for the angles, as those are
//absolute and don't need a zero switch.  If we do that they're seen as
//AnalogChannel s.  We'll want to write a quick wrapper class so we can get
//an angular offset, but we'll cross that bridge when we come to it.

//note: for two channel encoders, we need to specify a slot for both ports
//DIOs                                    SLOT     PORT
Encoder right_drive                     ( slot2,     1,
                                          slot2,     2 );
Encoder left_drive                      ( slot2,     3,
                                          slot2,     4 );
Encoder launcher_wheel                  ( slot2,     5,
                                          slot2,     6 );

DigitalInput launch_angle_switch        ( slot6,     1 );
Ultrasonic front_ultrasonic             ( slot6,     2,
                                          slot6,     3 );

//note: since we rely on the default value of kInches for the 5th arg
//we should use Ultrasonic::GetRangeInches().

//AIOs                                    SLOT     PORT
AnalogChannel launch_angle_pot          ( slot1,     1 );

//Relays                                  SLOT     PORT
Relay roller_spike_1                    ( slot6,     1 );
Relay roller_spike_2                    ( slot6,     2 );
Relay bridge_arm_spike                  ( slot6,     3 );

//USBs (on driver station)                         PORT
Joystick left_joystick_raw                         ( 1 );
Joystick right_joystick_raw                        ( 2 );
Joystick gunner_joystick_raw                       ( 3 );

joysmooth left_joystick        (  left_joystick_raw    );
joysmooth right_joystick       (  right_joystick_raw   );
joysmooth gunner_joystick      (  gunner_joystick_raw  );

//initialization of virtual devices:

//RobotDrive
RobotDrive drive (
    left_front_motor.jag,  //KEEP THIS ORDER (HERE)!  IT'S IMPORTANT!
    left_rear_motor.jag,   //LF, LR, RF, RR -- see documentation
    right_front_motor.jag, //for more details
    right_rear_motor.jag
);

//drive_jaguar                           JAGUAR&                 TYPE               REVERSE
drive_jaguar left_front_motor  =    { left_front_jag,  RobotDrive::kFrontLeftMotor,  true };
drive_jaguar left_rear_motor   =    { left_rear_jag,   RobotDrive::kRearLeftMotor,   true };
drive_jaguar right_front_motor =    { right_front_jag, RobotDrive::kFrontRightMotor, true };
drive_jaguar right_rear_motor  =    { right_rear_jag,  RobotDrive::kRearRightMotor,  true };
<|endoftext|>


<commit_before>#include "prime.h"

using namespace std;

int firstCandidate(unsigned long int currPrime, vector<int>& possPrime){
    for (int i = currPrime + 1; i < possPrime.size(); i++) {
        if (possPrime.at(i) == 1) {
            return i;
        }
    }

    return 0;
}

vector<unsigned long int> firstNPrime(unsigned long int n){

    vector<unsigned long int> primes;

    vector<int> possPrime;
    unsigned long int currPrime = 1;

    for (unsigned long int i = 0; i < n; i++) {
        possPrime.push_back(1);
    }
    possPrime.at(1) = 0;

    while (true) {
        currPrime = firstCandidate(currPrime,possPrime);

        if (currPrime == 0) {
            return primes;
        }

        primes.push_back(currPrime);

        for (int i = currPrime; i * currPrime < n; i++) {
            possPrime.at(i*currPrime) = 0;
        }
    }

    return primes;
}
<commit_msg>Code cleanup<commit_after>#include "prime.h"

using namespace std;

int firstCandidate(unsigned long int currPrime, vector<int>& possPrime){
    for (unsigned long int i = currPrime + 1; i < possPrime.size(); i++) {
        if (possPrime.at(i) == 1) {
            return i;
        }
    }

    return 0;
}

vector<unsigned long int> firstNPrime(unsigned long int n){

    vector<unsigned long int> primes;

    vector<int> possPrime;
    unsigned long int currPrime = 1;

    for (unsigned long int i = 0; i < n; i++) {
        possPrime.push_back(1);
    }
    possPrime.at(1) = 0;

    while (true) {
        currPrime = firstCandidate(currPrime,possPrime);

        if (currPrime == 0) {
            return primes;
        }

        primes.push_back(currPrime);

        for (int i = currPrime; i * currPrime < n; i++) {
            possPrime.at(i*currPrime) = 0;
        }
    }
}

// --------------------------------
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file PositionControl.cpp
 */

#include "PositionControl.hpp"
#include "ControlMath.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <ecl/geo/geo.h>

using namespace matrix;

void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
	_gain_vel_p = P;
	_gain_vel_i = I;
	_gain_vel_d = D;
}

void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down)
{
	_lim_vel_horizontal = vel_horizontal;
	_lim_vel_up = vel_up;
	_lim_vel_down = vel_down;
}

void PositionControl::setThrustLimits(const float min, const float max)
{
	// make sure there's always enough thrust vector length to infer the attitude
	_lim_thr_min = math::max(min, 10e-4f);
	_lim_thr_max = max;
}

void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
	_vel_int(2) += hover_thrust_new - _hover_thrust;
	setHoverThrust(hover_thrust_new);
}

void PositionControl::setState(const PositionControlStates &states)
{
	_pos = states.position;
	_vel = states.velocity;
	_yaw = states.yaw;
	_vel_dot = states.acceleration;
}

void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
	_acc_sp = Vector3f(setpoint.acceleration);
	_yaw_sp = setpoint.yaw;
	_yawspeed_sp = setpoint.yawspeed;
}

void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
{
	_constraints = constraints;

	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
	// constraints, then just use global constraints for the limits.
	if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) {
		_constraints.tilt = _lim_tilt;
	}

	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) {
		_constraints.speed_up = _lim_vel_up;
	}

	if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) {
		_constraints.speed_down = _lim_vel_down;
	}

	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
}

bool PositionControl::update(const float dt)
{
	// x and y input setpoints always have to come in pairs
	const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))
			   && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))
			   && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));

	_positionControl();
	_velocityControl(dt);

	_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
	_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control

	return valid && _updateSuccessful();
}

void PositionControl::_positionControl()
{
	// P-position controller
	Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
	// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
	ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
	// make sure there are no NAN elements for further reference while constraining
	ControlMath::setZeroIfNanVector3f(vel_sp_position);

	// Constrain horizontal velocity by prioritizing the velocity component along the
	// the desired position setpoint over the feed-forward term.
	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
	// Constrain velocity in z-direction.
	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

void PositionControl::_velocityControl(const float dt)
{
	// PID velocity control
	Vector3f vel_error = _vel_sp - _vel;
	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d);

	// For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion
	acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust;
	// No control input from setpoints or corresponding states which are NAN
	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);

	_accelerationControl();

	// Integrator anti-windup in vertical direction
	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
	    (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
		vel_error(2) = 0.f;
	}

	// Saturate maximal vertical thrust
	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);

	// Get allowed horizontal thrust after prioritizing vertical control
	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
	float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);

	// Saturate thrust in horizontal direction
	const Vector2f thrust_sp_xy(_thr_sp);
	const float thrust_sp_xy_norm = thrust_sp_xy.norm();

	if (thrust_sp_xy_norm > thrust_max_xy) {
		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
	}

	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
	const float arw_gain = 2.f / _gain_vel_p(0);
	vel_error.xy() = Vector2f(vel_error) - (arw_gain * (thrust_sp_xy - Vector2f(_thr_sp)));

	// Make sure integral doesn't get NAN
	ControlMath::setZeroIfNanVector3f(vel_error);
	// Update integral part of velocity control
	_vel_int += vel_error.emult(_gain_vel_i) * dt;

	// limit thrust integral
	_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2));
}

void PositionControl::_accelerationControl()
{
	// Assume standard acceleration due to gravity in vertical direction for attitude generation
	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
	// Scale thrust assuming hover thrust produces standard gravity
	float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
	// Project thrust to planned body attitude
	collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
	collective_thrust = math::min(collective_thrust, -_lim_thr_min);
	_thr_sp = body_z * collective_thrust;
}

bool PositionControl::_updateSuccessful()
{
	bool valid = true;

	// For each controlled state the estimate has to be valid
	for (int i = 0; i <= 2; i++) {
		if (PX4_ISFINITE(_pos_sp(i))) {
			valid = valid && PX4_ISFINITE(_pos(i));
		}

		if (PX4_ISFINITE(_vel_sp(i))) {
			valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
		}
	}

	// There has to be a valid output accleration and thrust setpoint otherwise there was no
	// setpoint-state pair for each axis that can get controlled
	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
	return valid;
}

void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
{
	local_position_setpoint.x = _pos_sp(0);
	local_position_setpoint.y = _pos_sp(1);
	local_position_setpoint.z = _pos_sp(2);
	local_position_setpoint.yaw = _yaw_sp;
	local_position_setpoint.yawspeed = _yawspeed_sp;
	local_position_setpoint.vx = _vel_sp(0);
	local_position_setpoint.vy = _vel_sp(1);
	local_position_setpoint.vz = _vel_sp(2);
	_acc_sp.copyTo(local_position_setpoint.acceleration);
	_thr_sp.copyTo(local_position_setpoint.thrust);
}

void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
{
	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}
<commit_msg>PositionControl: fix horizontal integral anti-windup<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file PositionControl.cpp
 */

#include "PositionControl.hpp"
#include "ControlMath.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <ecl/geo/geo.h>

using namespace matrix;

void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
{
	_gain_vel_p = P;
	_gain_vel_i = I;
	_gain_vel_d = D;
}

void PositionControl::setVelocityLimits(const float vel_horizontal, const float vel_up, const float vel_down)
{
	_lim_vel_horizontal = vel_horizontal;
	_lim_vel_up = vel_up;
	_lim_vel_down = vel_down;
}

void PositionControl::setThrustLimits(const float min, const float max)
{
	// make sure there's always enough thrust vector length to infer the attitude
	_lim_thr_min = math::max(min, 10e-4f);
	_lim_thr_max = max;
}

void PositionControl::updateHoverThrust(const float hover_thrust_new)
{
	_vel_int(2) += hover_thrust_new - _hover_thrust;
	setHoverThrust(hover_thrust_new);
}

void PositionControl::setState(const PositionControlStates &states)
{
	_pos = states.position;
	_vel = states.velocity;
	_yaw = states.yaw;
	_vel_dot = states.acceleration;
}

void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
{
	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
	_acc_sp = Vector3f(setpoint.acceleration);
	_yaw_sp = setpoint.yaw;
	_yawspeed_sp = setpoint.yawspeed;
}

void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
{
	_constraints = constraints;

	// For safety check if adjustable constraints are below global constraints. If they are not stricter than global
	// constraints, then just use global constraints for the limits.
	if (!PX4_ISFINITE(constraints.tilt) || (constraints.tilt > _lim_tilt)) {
		_constraints.tilt = _lim_tilt;
	}

	if (!PX4_ISFINITE(constraints.speed_up) || (constraints.speed_up > _lim_vel_up)) {
		_constraints.speed_up = _lim_vel_up;
	}

	if (!PX4_ISFINITE(constraints.speed_down) || (constraints.speed_down > _lim_vel_down)) {
		_constraints.speed_down = _lim_vel_down;
	}

	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
}

bool PositionControl::update(const float dt)
{
	// x and y input setpoints always have to come in pairs
	const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1)))
			   && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1)))
			   && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1)));

	_positionControl();
	_velocityControl(dt);

	_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
	_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control

	return valid && _updateSuccessful();
}

void PositionControl::_positionControl()
{
	// P-position controller
	Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);
	// Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence
	ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);
	// make sure there are no NAN elements for further reference while constraining
	ControlMath::setZeroIfNanVector3f(vel_sp_position);

	// Constrain horizontal velocity by prioritizing the velocity component along the
	// the desired position setpoint over the feed-forward term.
	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
	// Constrain velocity in z-direction.
	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

void PositionControl::_velocityControl(const float dt)
{
	// PID velocity control
	Vector3f vel_error = _vel_sp - _vel;
	Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int + _vel_dot.emult(_gain_vel_d);

	// For backwards compatibility of the gains to non-acceleration based control, needs to be overcome with configuration conversion
	acc_sp_velocity *= CONSTANTS_ONE_G / _hover_thrust;
	// No control input from setpoints or corresponding states which are NAN
	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);

	_accelerationControl();

	// Integrator anti-windup in vertical direction
	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
	    (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
		vel_error(2) = 0.f;
	}

	// Saturate maximal vertical thrust
	_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);

	// Get allowed horizontal thrust after prioritizing vertical control
	const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
	const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
	float thrust_max_xy = sqrtf(thrust_max_squared - thrust_z_squared);

	// Saturate thrust in horizontal direction
	const Vector2f thrust_sp_xy(_thr_sp);
	const float thrust_sp_xy_norm = thrust_sp_xy.norm();

	if (thrust_sp_xy_norm > thrust_max_xy) {
		_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
	}

	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
	const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
	const float arw_gain = 2.f / (_gain_vel_p(0) * (CONSTANTS_ONE_G / _hover_thrust));
	vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));

	// Make sure integral doesn't get NAN
	ControlMath::setZeroIfNanVector3f(vel_error);
	// Update integral part of velocity control
	_vel_int += vel_error.emult(_gain_vel_i) * dt;

	// limit thrust integral
	_vel_int(2) = math::min(fabsf(_vel_int(2)), _lim_thr_max) * sign(_vel_int(2));
}

void PositionControl::_accelerationControl()
{
	// Assume standard acceleration due to gravity in vertical direction for attitude generation
	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);
	// Scale thrust assuming hover thrust produces standard gravity
	float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
	// Project thrust to planned body attitude
	collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
	collective_thrust = math::min(collective_thrust, -_lim_thr_min);
	_thr_sp = body_z * collective_thrust;
}

bool PositionControl::_updateSuccessful()
{
	bool valid = true;

	// For each controlled state the estimate has to be valid
	for (int i = 0; i <= 2; i++) {
		if (PX4_ISFINITE(_pos_sp(i))) {
			valid = valid && PX4_ISFINITE(_pos(i));
		}

		if (PX4_ISFINITE(_vel_sp(i))) {
			valid = valid && PX4_ISFINITE(_vel(i)) && PX4_ISFINITE(_vel_dot(i));
		}
	}

	// There has to be a valid output accleration and thrust setpoint otherwise there was no
	// setpoint-state pair for each axis that can get controlled
	valid = valid && PX4_ISFINITE(_acc_sp(0)) && PX4_ISFINITE(_acc_sp(1)) && PX4_ISFINITE(_acc_sp(2));
	valid = valid && PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1)) && PX4_ISFINITE(_thr_sp(2));
	return valid;
}

void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint) const
{
	local_position_setpoint.x = _pos_sp(0);
	local_position_setpoint.y = _pos_sp(1);
	local_position_setpoint.z = _pos_sp(2);
	local_position_setpoint.yaw = _yaw_sp;
	local_position_setpoint.yawspeed = _yawspeed_sp;
	local_position_setpoint.vx = _vel_sp(0);
	local_position_setpoint.vy = _vel_sp(1);
	local_position_setpoint.vz = _vel_sp(2);
	_acc_sp.copyTo(local_position_setpoint.acceleration);
	_thr_sp.copyTo(local_position_setpoint.thrust);
}

void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
{
	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}
<|endoftext|>


<commit_before>/**
  Licensed to the Apache Software Foundation (ASF) under one
  or more contributor license agreements.  See the NOTICE file
  distributed with this work for additional information
  regarding copyright ownership.  The ASF licenses this file
  to you under the Apache License, Version 2.0 (the
  "License"); you may not use this file except in compliance
  with the License.  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
 */

#include <sstream>
#include <iostream>
#include <string_view>
#include "tscpp/api/PluginInit.h"
#include "tscpp/api/GlobalPlugin.h"
#include "tscpp/api/TransformationPlugin.h"
#include "tscpp/api/Logger.h"

#include <Magick++.h>

using std::string;
using namespace Magick;
using namespace atscppapi;

#define TAG "webp_transform"

namespace
{
GlobalPlugin *plugin;
}

class ImageTransform : public TransformationPlugin
{
public:
  ImageTransform(Transaction &transaction) : TransformationPlugin(transaction, TransformationPlugin::RESPONSE_TRANSFORMATION)
  {
    TransformationPlugin::registerHook(HOOK_READ_RESPONSE_HEADERS);
  }

  void
  handleReadResponseHeaders(Transaction &transaction) override
  {
    transaction.getServerResponse().getHeaders()["Content-Type"] = "image/webp";
    transaction.getServerResponse().getHeaders()["Vary"]         = "Content-Type"; // to have a separate cache entry.

    TS_DEBUG(TAG, "url %s", transaction.getServerRequest().getUrl().getUrlString().c_str());
    transaction.resume();
  }

  void
  consume(std::string_view data) override
  {
    _img.write(data.data(), data.length());
  }

  void
  handleInputComplete() override
  {
    string input_data = _img.str();
    Blob input_blob(input_data.data(), input_data.length());
    Image image;
    image.read(input_blob);

    Blob output_blob;
    image.magick("WEBP");
    image.write(&output_blob);
    produce(std::string_view(reinterpret_cast<const char *>(output_blob.data()), output_blob.length()));

    setOutputComplete();
  }

  ~ImageTransform() override {}

private:
  std::stringstream _img;
};

class GlobalHookPlugin : public GlobalPlugin
{
public:
  GlobalHookPlugin() { registerHook(HOOK_READ_RESPONSE_HEADERS); }
  void
  handleReadResponseHeaders(Transaction &transaction) override
  {
    string ctype      = transaction.getServerResponse().getHeaders().values("Content-Type");
    string user_agent = transaction.getServerRequest().getHeaders().values("User-Agent");
    if (user_agent.find("Chrome") != string::npos && (ctype.find("jpeg") != string::npos || ctype.find("png") != string::npos)) {
      TS_DEBUG(TAG, "Content type is either jpeg or png. Converting to webp");
      transaction.addPlugin(new ImageTransform(transaction));
    }

    transaction.resume();
  }
};

void
TSPluginInit(int argc ATSCPPAPI_UNUSED, const char *argv[] ATSCPPAPI_UNUSED)
{
  if (!RegisterGlobalPlugin("CPP_Webp_Transform", "apache", "dev@trafficserver.apache.org")) {
    return;
  }
  InitializeMagick("");
  plugin = new GlobalHookPlugin();
}
<commit_msg>Transform images when a client accepts image/webp.<commit_after>/**
  Licensed to the Apache Software Foundation (ASF) under one
  or more contributor license agreements.  See the NOTICE file
  distributed with this work for additional information
  regarding copyright ownership.  The ASF licenses this file
  to you under the Apache License, Version 2.0 (the
  "License"); you may not use this file except in compliance
  with the License.  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
 */

#include <sstream>
#include <iostream>
#include <string_view>
#include "tscpp/api/PluginInit.h"
#include "tscpp/api/GlobalPlugin.h"
#include "tscpp/api/TransformationPlugin.h"
#include "tscpp/api/Logger.h"

#include <Magick++.h>

using std::string;
using namespace Magick;
using namespace atscppapi;

#define TAG "webp_transform"

namespace
{
GlobalPlugin *plugin;
}

class ImageTransform : public TransformationPlugin
{
public:
  ImageTransform(Transaction &transaction) : TransformationPlugin(transaction, TransformationPlugin::RESPONSE_TRANSFORMATION)
  {
    TransformationPlugin::registerHook(HOOK_READ_RESPONSE_HEADERS);
  }

  void
  handleReadResponseHeaders(Transaction &transaction) override
  {
    transaction.getServerResponse().getHeaders()["Content-Type"] = "image/webp";
    transaction.getServerResponse().getHeaders()["Vary"]         = "Content-Type"; // to have a separate cache entry.

    TS_DEBUG(TAG, "url %s", transaction.getServerRequest().getUrl().getUrlString().c_str());
    transaction.resume();
  }

  void
  consume(std::string_view data) override
  {
    _img.write(data.data(), data.length());
  }

  void
  handleInputComplete() override
  {
    string input_data = _img.str();
    Blob input_blob(input_data.data(), input_data.length());
    Image image;
    image.read(input_blob);

    Blob output_blob;
    image.magick("WEBP");
    image.write(&output_blob);
    produce(std::string_view(reinterpret_cast<const char *>(output_blob.data()), output_blob.length()));

    setOutputComplete();
  }

  ~ImageTransform() override {}

private:
  std::stringstream _img;
};

class GlobalHookPlugin : public GlobalPlugin
{
public:
  GlobalHookPlugin() { registerHook(HOOK_READ_RESPONSE_HEADERS); }
  void
  handleReadResponseHeaders(Transaction &transaction) override
  {
    string ctype      = transaction.getServerResponse().getHeaders().values("Content-Type");
    string user_agent = transaction.getServerRequest().getHeaders().values("User-Agent");
    string accept     = transaction.getServerRequest().getHeaders().values("Accept");

    bool webp_supported = accept.find("image/webp") != string::npos || user_agent.find("Chrome") != string::npos;
    bool image_format   = ctype.find("jpeg") != string::npos || ctype.find("png") != string::npos;

    if (webp_supported && image_format) {
      TS_DEBUG(TAG, "Content type is either jpeg or png. Converting to webp");
      transaction.addPlugin(new ImageTransform(transaction));
    }

    transaction.resume();
  }
};

void
TSPluginInit(int argc ATSCPPAPI_UNUSED, const char *argv[] ATSCPPAPI_UNUSED)
{
  if (!RegisterGlobalPlugin("CPP_Webp_Transform", "apache", "dev@trafficserver.apache.org")) {
    return;
  }
  InitializeMagick("");
  plugin = new GlobalHookPlugin();
}
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California
 *    Jan Issac (jan.issac@gmail.com)
 *    Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * @date 2014
 * @author Jan Issac (jan.issac@gmail.com)
 * @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * Max-Planck-Institute for Intelligent Systems, University of Southern California
 */

#ifndef FAST_FILTERING_FILTERS_DETERMINISTIC_KALMAN_FILTER_HPP
#define FAST_FILTERING_FILTERS_DETERMINISTIC_KALMAN_FILTER_HPP

#include <fast_filtering/utils/traits.hpp>

#include <boost/shared_ptr.hpp>

#include <fast_filtering/distributions/gaussian.hpp>
#include <fast_filtering/models/process_models/linear_process_model.hpp>
#include <fast_filtering/models/observation_models/linear_observation_model.hpp>

namespace ff
{

template <typename ProcessModel, typename ObservationModel>
class KalmanFilter
{
public:
    typedef boost::shared_ptr<ProcessModel> ProcessModelPtr;
    typedef boost::shared_ptr<ObservationModel> ObserationModelPtr;

    typedef typename ProcessModel::Scalar Scalar;
    typedef typename ProcessModel::State State;
    typedef typename ProcessModel::Input Input;
    typedef typename ProcessModel::State State;
    typedef typename ProcessModel::DynamicsMatrix DynamicsMatrix;
    typedef typename ProcessModel::Operator DynamicsCovariance;

    typedef typename ObservationModel::Observation Observation;
    typedef typename ObservationModel::SensorMatrix SensorMatrix;
    typedef typename ObservationModel::Operator SensorCovariance;

    typedef Gaussian<State> StateDistribution;

public:
    KalmanFilter(const ProcessModelPtr& process_model,
                 const ProcessModelPtr& observation_model):
        process_model_(process_model),
        observation_model_(observation_model)
    {
    }

    void Predict(const double delta_time,
                 const StateDistribution& prior,
                 StateDistribution& predicted)
    {
        const DynamicsMatrix& A = process_model_->A();
        const DynamicsCovariance Q = delta_time * process_model_->Covariance();

        predicted.Mean(A * prior.Mean());
        predicted.Covariance(A * prior.Covariance() * A.transpose() + Q);
    }

    void Update(const StateDistribution& predicted,
                const Observation& y,
                StateDistribution& posterior)
    {
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

        const SensorMatrix& H = observation_model_->H();
        const SensorMatrix R = observation_model_->Covariance();

        const Matrix S = H * predicted.Covariance() * H.transpose() + R;
        const Matrix K = predicted.Covariance() * H.transpose() * S.inverse();

        posterior.Mean(
                    predicted.Mean() + K * (y - H * predicted.Mean()));
        posterior.Covariance(
                    predicted.Covariance() - K * H * predicted.Covariance());
    }

protected:
    ProcessModelPtr process_model_;
    ObserationModelPtr observation_model_;
};

}

#endif
<commit_msg>Minor KalmanFilter fixes<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California
 *    Jan Issac (jan.issac@gmail.com)
 *    Manuel Wuthrich (manuel.wuthrich@gmail.com)
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * @date 2014
 * @author Jan Issac (jan.issac@gmail.com)
 * @author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * Max-Planck-Institute for Intelligent Systems, University of Southern California
 */

#ifndef FAST_FILTERING_FILTERS_DETERMINISTIC_KALMAN_FILTER_HPP
#define FAST_FILTERING_FILTERS_DETERMINISTIC_KALMAN_FILTER_HPP

#include <fast_filtering/utils/traits.hpp>

#include <boost/shared_ptr.hpp>

#include <fast_filtering/distributions/gaussian.hpp>
#include <fast_filtering/models/process_models/linear_process_model.hpp>
#include <fast_filtering/models/observation_models/linear_observation_model.hpp>

namespace ff
{

template <typename ProcessModel, typename ObservationModel>
class KalmanFilter
{
public:
    typedef boost::shared_ptr<ProcessModel> ProcessModelPtr;
    typedef boost::shared_ptr<ObservationModel> ObservationModelPtr;

    typedef typename ProcessModel::Scalar Scalar;
    typedef typename ProcessModel::State State;
    typedef typename ProcessModel::Input Input;
    typedef typename ProcessModel::DynamicsMatrix DynamicsMatrix;
    typedef typename ProcessModel::Operator DynamicsCovariance;

    typedef typename ObservationModel::Observation Observation;
    typedef typename ObservationModel::SensorMatrix SensorMatrix;
    typedef typename ObservationModel::Operator SensorCovariance;

    typedef Gaussian<State> StateDistribution;

public:
    KalmanFilter(const ProcessModelPtr& process_model,
                 const ObservationModelPtr& observation_model):
        process_model_(process_model),
        observation_model_(observation_model)
    {
    }

    virtual ~KalmanFilter() { }

    void Predict(const double delta_time,
                 const StateDistribution& prior,
                 StateDistribution& predicted)
    {
        const DynamicsMatrix& A = process_model_->A();
        const DynamicsCovariance Q = delta_time * process_model_->Covariance();
        predicted.Mean(A * prior.Mean());
        predicted.Covariance(A * prior.Covariance() * A.transpose() + Q);
    }

    void Update(const StateDistribution& predicted,
                const Observation& y,
                StateDistribution& posterior)
    {
        typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Matrix;

        const SensorMatrix& H = observation_model_->H();
        const SensorMatrix R = observation_model_->Covariance();

        const Matrix S = H * predicted.Covariance() * H.transpose() + R;
        const Matrix K = predicted.Covariance() * H.transpose() * S.inverse();

        posterior.Mean(
                    predicted.Mean() + K * (y - H * predicted.Mean()));
        posterior.Covariance(
                    predicted.Covariance() - K * H * predicted.Covariance());
    }

protected:
    ProcessModelPtr process_model_;
    ObservationModelPtr observation_model_;
};

}

#endif
<|endoftext|>