<commit_before>/******************************************************************************
 * Copyright 2017 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 reference_line.cc
 **/

#include "modules/planning/reference_line/reference_line.h"

#include <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "boost/math/tools/minima.hpp"

#include "modules/common/log.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/string_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"

namespace apollo {
namespace planning {

using MapPath = hdmap::Path;
using SLPoint = apollo::common::SLPoint;

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {}

ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0,
        0.0, 0.0);
  }
}

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points,
    const std::vector<hdmap::LaneSegment>& lane_segments,
    const double max_approximation_error)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
                            reference_points.begin(), reference_points.end()),
                        lane_segments, max_approximation_error)) {}

ReferencePoint ReferenceLine::get_reference_point(const double s) const {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front()) {
    AWARN << "The requested s is before the start point of the reference "
             "line; reference line starts at "
          << accumulated_s.front() << ", requested " << s << ".";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.front().lane_waypoints());
    }
    return ref_point;
  }
  if (s > accumulated_s.back()) {
    AWARN << "The requested s exceeds the reference line; reference line "
             "ends at "
          << accumulated_s.back() << "requested " << s << " .";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.back().lane_waypoints());
    }
    return ref_point;
  }

  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  if (it_lower == accumulated_s.begin()) {
    return reference_points_.front();
  } else {
    std::uint32_t index =
        static_cast<std::uint32_t>(it_lower - accumulated_s.begin());
    auto p0 = reference_points_[index - 1];
    auto p1 = reference_points_[index];

    auto s0 = accumulated_s[index - 1];
    auto s1 = accumulated_s[index];

    return interpolate(p0, s0, p1, s1, s);
  }
}

double ReferenceLine::find_min_distance_point(const ReferencePoint& p0,
                                              const double s0,
                                              const ReferencePoint& p1,
                                              const double s1, const double x,
                                              const double y) {
  auto func_dist_square = [&p0, &p1, &s0, &s1, &x, &y](const double s) {
    auto p = interpolate(p0, s0, p1, s1, s);
    double dx = p.x() - x;
    double dy = p.y() - y;
    return dx * dx + dy * dy;
  };

  return ::boost::math::tools::brent_find_minima(func_dist_square, s0, s1, 8)
      .first;
}

ReferencePoint ReferenceLine::get_reference_point(const double x,
                                                  const double y) const {
  CHECK_GE(reference_points_.size(), 0);

  auto func_distance_square = [](const ReferencePoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  double d_min = func_distance_square(reference_points_.front(), x, y);
  double index_min = 0;

  for (uint32_t i = 1; i < reference_points_.size(); ++i) {
    double d_temp = func_distance_square(reference_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }

  uint32_t index_start = (index_min == 0 ? index_min : index_min - 1);
  uint32_t index_end =
      (index_min + 1 == reference_points_.size() ? index_min : index_min + 1);

  if (index_start == index_end) {
    return reference_points_[index_start];
  }

  double s0 = map_path_.accumulated_s()[index_start];
  double s1 = map_path_.accumulated_s()[index_end];

  double s = ReferenceLine::find_min_distance_point(
      reference_points_[index_start], s0, reference_points_[index_end], s1, x,
      y);

  return interpolate(reference_points_[index_start], s0,
                     reference_points_[index_end], s1, s);
}

bool ReferenceLine::SLToXY(const common::SLPoint& sl_point,
                           common::math::Vec2d* const xy_point) const {
  CHECK_NOTNULL(xy_point);
  if (map_path_.num_points() < 2) {
    AERROR << "The reference line has too few points.";
    return false;
  }

  const auto matched_point = get_reference_point(sl_point.s());
  const auto angle = common::math::Angle16::from_rad(matched_point.heading());
  xy_point->set_x(matched_point.x() - common::math::sin(angle) * sl_point.l());
  xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l());
  return true;
}

bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point,
                           common::SLPoint* const sl_point) const {
  DCHECK_NOTNULL(sl_point);
  double s = 0;
  double l = 0;
  if (!map_path_.get_projection(xy_point, &s, &l)) {
    AERROR << "Can't get nearest point from path.";
    return false;
  }

  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

ReferencePoint ReferenceLine::interpolate(const ReferencePoint& p0,
                                          const double s0,
                                          const ReferencePoint& p1,
                                          const double s1, const double s) {
  if (std::fabs(s0 - s1) < common::math::kMathEpsilon) {
    return p0;
  }
  DCHECK_LE(s0 - 1.0e-6, s) << " s: " << s << " is less than s0 :" << s0;
  DCHECK_LE(s, s1 + 1.0e-6) << "s: " << s << "is larger than s1: " << s1;

  CHECK(!p0.lane_waypoints().empty());
  CHECK(!p1.lane_waypoints().empty());
  const double x = common::math::lerp(p0.x(), s0, p1.x(), s1, s);
  const double y = common::math::lerp(p0.y(), s0, p1.y(), s1, s);
  const double heading =
      common::math::slerp(p0.heading(), s0, p1.heading(), s1, s);
  const double kappa = common::math::lerp(p0.kappa(), s0, p1.kappa(), s1, s);
  const double dkappa = common::math::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s);
  const auto& p0_waypoint = p0.lane_waypoints()[0];
  std::vector<hdmap::LaneWaypoint> waypoints;
  double upper_bound = 0.0;
  double lower_bound = 0.0;
  if ((s - s0) + p0_waypoint.s <= p0_waypoint.lane->total_length()) {
    const double lane_s = p0_waypoint.s + s - s0;
    waypoints.emplace_back(p0_waypoint.lane, lane_s);
    p0_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  const auto& p1_waypoint = p1.lane_waypoints()[0];
  if (p1_waypoint.lane->id().id() != p0_waypoint.lane->id().id() &&
      p1_waypoint.s - (s1 - s) >= 0) {
    const double lane_s = p1_waypoint.s - (s1 - s);
    waypoints.emplace_back(p1_waypoint.lane, lane_s);
    p1_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }

  return ReferencePoint(hdmap::MapPathPoint({x, y}, heading, waypoints), kappa,
                        dkappa, lower_bound, upper_bound);
}

const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
  return reference_points_;
}

const MapPath& ReferenceLine::map_path() const { return map_path_; }

bool ReferenceLine::get_lane_width(const double s, double* const left_width,
                                   double* const right_width) const {
  return map_path_.get_width(s, left_width, right_width);
}

bool ReferenceLine::is_on_road(const common::SLPoint& sl_point) const {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double left_width = 0.0;
  double right_width = 0.0;

  if (!get_lane_width(sl_point.s(), &left_width, &right_width)) {
    return false;
  }

  if (sl_point.l() <= -right_width || sl_point.l() >= left_width) {
    return false;
  }

  return true;
}

bool ReferenceLine::GetSLBoundary(const common::math::Box2d& box,
                                  SLBoundary* const sl_boundary) const {
  double start_s(std::numeric_limits<double>::max());
  double end_s(std::numeric_limits<double>::lowest());
  double start_l(std::numeric_limits<double>::max());
  double end_l(std::numeric_limits<double>::lowest());
  std::vector<common::math::Vec2d> corners;
  box.GetAllCorners(&corners);
  for (const auto& point : corners) {
    common::SLPoint sl_point;
    if (!XYToSL(point, &sl_point)) {
      AERROR << "failed to get projection for point: " << point.DebugString()
             << " on reference line.";
      return false;
    }
    start_s = std::fmin(start_s, sl_point.s());
    end_s = std::fmax(end_s, sl_point.s());
    start_l = std::fmin(start_l, sl_point.l());
    end_l = std::fmax(end_l, sl_point.l());
  }
  sl_boundary->set_start_s(start_s);
  sl_boundary->set_end_s(end_s);
  sl_boundary->set_start_l(start_l);
  sl_boundary->set_end_l(end_l);
  return true;
}

bool ReferenceLine::HasOverlap(const common::math::Box2d& box) const {
  SLBoundary sl_boundary;
  if (!GetSLBoundary(box, &sl_boundary)) {
    AERROR << "Failed to get sl boundary for box " << box.DebugString();
    return false;
  }
  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > length()) {
    return false;
  }
  if (sl_boundary.start_l() * sl_boundary.end_l() < 0) {
    return false;
  }

  double left_width = 0.0;
  double right_width = 0.0;
  const double mid_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
  if (!map_path_.get_width(mid_s, &left_width, &right_width)) {
    AERROR << "failed to get width at s = " << mid_s;
    return false;
  }
  if (sl_boundary.start_l() > 0) {
    return sl_boundary.start_l() < left_width;
  } else {
    return sl_boundary.end_l() > -right_width;
  }
}

std::string ReferenceLine::DebugString() const {
  const auto limit =
      std::min(reference_points_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
  return apollo::common::util::StrCat(
      "point num:", reference_points_.size(),
      apollo::common::util::PrintDebugStringIter(
          reference_points_.begin(), reference_points_.begin() + limit, ""));
}

double ReferenceLine::GetSpeedLimitFromS(const double s) const {
  const auto& map_path_point = get_reference_point(s);
  double speed_limit = FLAGS_planning_speed_upper_limit;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr";
      continue;
    }
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }
  return speed_limit;
}

double ReferenceLine::GetSpeedLimitFromPoint(
    const common::math::Vec2d& point) const {
  SLPoint sl;
  if (!XYToSL(point, &sl)) {
    AWARN << "Failed to get projection for point: " << point.DebugString();
    return FLAGS_planning_speed_upper_limit;
  }
  return GetSpeedLimitFromS(sl.s());
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: make sure all reference points have lane way point for width calculation. it fixes one planning crash.<commit_after>/******************************************************************************
 * Copyright 2017 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 reference_line.cc
 **/

#include "modules/planning/reference_line/reference_line.h"

#include <algorithm>
#include <limits>
#include <string>
#include <utility>
#include <vector>

#include "boost/math/tools/minima.hpp"

#include "modules/common/log.h"
#include "modules/common/math/angle.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/util/string_util.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/double.h"

namespace apollo {
namespace planning {

using MapPath = hdmap::Path;
using SLPoint = apollo::common::SLPoint;

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
          reference_points.begin(), reference_points.end()))) {}

ReferenceLine::ReferenceLine(const MapPath& hdmap_path)
    : map_path_(hdmap_path) {
  for (const auto& point : hdmap_path.path_points()) {
    DCHECK(!point.lane_waypoints().empty());
    const auto& lane_waypoint = point.lane_waypoints()[0];
    reference_points_.emplace_back(
        hdmap::MapPathPoint(point, point.heading(), lane_waypoint), 0.0, 0.0,
        0.0, 0.0);
  }
}

ReferenceLine::ReferenceLine(
    const std::vector<ReferencePoint>& reference_points,
    const std::vector<hdmap::LaneSegment>& lane_segments,
    const double max_approximation_error)
    : reference_points_(reference_points),
      map_path_(MapPath(std::vector<hdmap::MapPathPoint>(
                            reference_points.begin(), reference_points.end()),
                        lane_segments, max_approximation_error)) {}

ReferencePoint ReferenceLine::get_reference_point(const double s) const {
  const auto& accumulated_s = map_path_.accumulated_s();
  if (s < accumulated_s.front()) {
    AWARN << "The requested s is before the start point of the reference "
             "line; reference line starts at "
          << accumulated_s.front() << ", requested " << s << ".";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.front().lane_waypoints());
    }
    return ref_point;
  }
  if (s > accumulated_s.back()) {
    AWARN << "The requested s exceeds the reference line; reference line "
             "ends at "
          << accumulated_s.back() << "requested " << s << " .";
    ReferencePoint ref_point(map_path_.get_smooth_point(s), 0.0, 0.0, 0.0, 0.0);
    if (ref_point.lane_waypoints().empty()) {
      ref_point.add_lane_waypoints(reference_points_.back().lane_waypoints());
    }
    return ref_point;
  }

  auto it_lower =
      std::lower_bound(accumulated_s.begin(), accumulated_s.end(), s);
  if (it_lower == accumulated_s.begin()) {
    return reference_points_.front();
  } else {
    std::uint32_t index =
        static_cast<std::uint32_t>(it_lower - accumulated_s.begin());
    auto p0 = reference_points_[index - 1];
    auto p1 = reference_points_[index];

    auto s0 = accumulated_s[index - 1];
    auto s1 = accumulated_s[index];

    return interpolate(p0, s0, p1, s1, s);
  }
}

double ReferenceLine::find_min_distance_point(const ReferencePoint& p0,
                                              const double s0,
                                              const ReferencePoint& p1,
                                              const double s1, const double x,
                                              const double y) {
  auto func_dist_square = [&p0, &p1, &s0, &s1, &x, &y](const double s) {
    auto p = interpolate(p0, s0, p1, s1, s);
    double dx = p.x() - x;
    double dy = p.y() - y;
    return dx * dx + dy * dy;
  };

  return ::boost::math::tools::brent_find_minima(func_dist_square, s0, s1, 8)
      .first;
}

ReferencePoint ReferenceLine::get_reference_point(const double x,
                                                  const double y) const {
  CHECK_GE(reference_points_.size(), 0);

  auto func_distance_square = [](const ReferencePoint& point, const double x,
                                 const double y) {
    double dx = point.x() - x;
    double dy = point.y() - y;
    return dx * dx + dy * dy;
  };

  double d_min = func_distance_square(reference_points_.front(), x, y);
  double index_min = 0;

  for (uint32_t i = 1; i < reference_points_.size(); ++i) {
    double d_temp = func_distance_square(reference_points_[i], x, y);
    if (d_temp < d_min) {
      d_min = d_temp;
      index_min = i;
    }
  }

  uint32_t index_start = (index_min == 0 ? index_min : index_min - 1);
  uint32_t index_end =
      (index_min + 1 == reference_points_.size() ? index_min : index_min + 1);

  if (index_start == index_end) {
    return reference_points_[index_start];
  }

  double s0 = map_path_.accumulated_s()[index_start];
  double s1 = map_path_.accumulated_s()[index_end];

  double s = ReferenceLine::find_min_distance_point(
      reference_points_[index_start], s0, reference_points_[index_end], s1, x,
      y);

  return interpolate(reference_points_[index_start], s0,
                     reference_points_[index_end], s1, s);
}

bool ReferenceLine::SLToXY(const common::SLPoint& sl_point,
                           common::math::Vec2d* const xy_point) const {
  CHECK_NOTNULL(xy_point);
  if (map_path_.num_points() < 2) {
    AERROR << "The reference line has too few points.";
    return false;
  }

  const auto matched_point = get_reference_point(sl_point.s());
  const auto angle = common::math::Angle16::from_rad(matched_point.heading());
  xy_point->set_x(matched_point.x() - common::math::sin(angle) * sl_point.l());
  xy_point->set_y(matched_point.y() + common::math::cos(angle) * sl_point.l());
  return true;
}

bool ReferenceLine::XYToSL(const common::math::Vec2d& xy_point,
                           common::SLPoint* const sl_point) const {
  DCHECK_NOTNULL(sl_point);
  double s = 0;
  double l = 0;
  if (!map_path_.get_projection(xy_point, &s, &l)) {
    AERROR << "Can't get nearest point from path.";
    return false;
  }

  sl_point->set_s(s);
  sl_point->set_l(l);
  return true;
}

ReferencePoint ReferenceLine::interpolate(const ReferencePoint& p0,
                                          const double s0,
                                          const ReferencePoint& p1,
                                          const double s1, const double s) {
  if (std::fabs(s0 - s1) < common::math::kMathEpsilon) {
    return p0;
  }
  DCHECK_LE(s0 - 1.0e-6, s) << " s: " << s << " is less than s0 :" << s0;
  DCHECK_LE(s, s1 + 1.0e-6) << "s: " << s << "is larger than s1: " << s1;

  CHECK(!p0.lane_waypoints().empty());
  CHECK(!p1.lane_waypoints().empty());
  const double x = common::math::lerp(p0.x(), s0, p1.x(), s1, s);
  const double y = common::math::lerp(p0.y(), s0, p1.y(), s1, s);
  const double heading =
      common::math::slerp(p0.heading(), s0, p1.heading(), s1, s);
  const double kappa = common::math::lerp(p0.kappa(), s0, p1.kappa(), s1, s);
  const double dkappa = common::math::lerp(p0.dkappa(), s0, p1.dkappa(), s1, s);
  const auto& p0_waypoint = p0.lane_waypoints()[0];
  std::vector<hdmap::LaneWaypoint> waypoints;
  double upper_bound = 0.0;
  double lower_bound = 0.0;
  if ((s - s0) + p0_waypoint.s <= p0_waypoint.lane->total_length()) {
    const double lane_s = p0_waypoint.s + s - s0;
    waypoints.emplace_back(p0_waypoint.lane, lane_s);
    p0_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  const auto& p1_waypoint = p1.lane_waypoints()[0];
  if (p1_waypoint.lane->id().id() != p0_waypoint.lane->id().id() &&
      p1_waypoint.s - (s1 - s) >= 0) {
    const double lane_s = p1_waypoint.s - (s1 - s);
    waypoints.emplace_back(p1_waypoint.lane, lane_s);
    p1_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  if (waypoints.empty()) {
    const double lane_s = p0_waypoint.s;
    waypoints.emplace_back(p0_waypoint.lane, lane_s);
    p0_waypoint.lane->get_width(lane_s, &upper_bound, &lower_bound);
  }
  return ReferencePoint(hdmap::MapPathPoint({x, y}, heading, waypoints), kappa,
                        dkappa, lower_bound, upper_bound);
}

const std::vector<ReferencePoint>& ReferenceLine::reference_points() const {
  return reference_points_;
}

const MapPath& ReferenceLine::map_path() const { return map_path_; }

bool ReferenceLine::get_lane_width(const double s, double* const left_width,
                                   double* const right_width) const {
  return map_path_.get_width(s, left_width, right_width);
}

bool ReferenceLine::is_on_road(const common::SLPoint& sl_point) const {
  if (sl_point.s() <= 0 || sl_point.s() > map_path_.length()) {
    return false;
  }
  double left_width = 0.0;
  double right_width = 0.0;

  if (!get_lane_width(sl_point.s(), &left_width, &right_width)) {
    return false;
  }

  if (sl_point.l() <= -right_width || sl_point.l() >= left_width) {
    return false;
  }

  return true;
}

bool ReferenceLine::GetSLBoundary(const common::math::Box2d& box,
                                  SLBoundary* const sl_boundary) const {
  double start_s(std::numeric_limits<double>::max());
  double end_s(std::numeric_limits<double>::lowest());
  double start_l(std::numeric_limits<double>::max());
  double end_l(std::numeric_limits<double>::lowest());
  std::vector<common::math::Vec2d> corners;
  box.GetAllCorners(&corners);
  for (const auto& point : corners) {
    common::SLPoint sl_point;
    if (!XYToSL(point, &sl_point)) {
      AERROR << "failed to get projection for point: " << point.DebugString()
             << " on reference line.";
      return false;
    }
    start_s = std::fmin(start_s, sl_point.s());
    end_s = std::fmax(end_s, sl_point.s());
    start_l = std::fmin(start_l, sl_point.l());
    end_l = std::fmax(end_l, sl_point.l());
  }
  sl_boundary->set_start_s(start_s);
  sl_boundary->set_end_s(end_s);
  sl_boundary->set_start_l(start_l);
  sl_boundary->set_end_l(end_l);
  return true;
}

bool ReferenceLine::HasOverlap(const common::math::Box2d& box) const {
  SLBoundary sl_boundary;
  if (!GetSLBoundary(box, &sl_boundary)) {
    AERROR << "Failed to get sl boundary for box " << box.DebugString();
    return false;
  }
  if (sl_boundary.end_s() < 0 || sl_boundary.start_s() > length()) {
    return false;
  }
  if (sl_boundary.start_l() * sl_boundary.end_l() < 0) {
    return false;
  }

  double left_width = 0.0;
  double right_width = 0.0;
  const double mid_s = (sl_boundary.start_s() + sl_boundary.end_s()) / 2.0;
  if (!map_path_.get_width(mid_s, &left_width, &right_width)) {
    AERROR << "failed to get width at s = " << mid_s;
    return false;
  }
  if (sl_boundary.start_l() > 0) {
    return sl_boundary.start_l() < left_width;
  } else {
    return sl_boundary.end_l() > -right_width;
  }
}

std::string ReferenceLine::DebugString() const {
  const auto limit =
      std::min(reference_points_.size(),
               static_cast<size_t>(FLAGS_trajectory_point_num_for_debug));
  return apollo::common::util::StrCat(
      "point num:", reference_points_.size(),
      apollo::common::util::PrintDebugStringIter(
          reference_points_.begin(), reference_points_.begin() + limit, ""));
}

double ReferenceLine::GetSpeedLimitFromS(const double s) const {
  const auto& map_path_point = get_reference_point(s);
  double speed_limit = FLAGS_planning_speed_upper_limit;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr";
      continue;
    }
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }
  return speed_limit;
}

double ReferenceLine::GetSpeedLimitFromPoint(
    const common::math::Vec2d& point) const {
  SLPoint sl;
  if (!XYToSL(point, &sl)) {
    AWARN << "Failed to get projection for point: " << point.DebugString();
    return FLAGS_planning_speed_upper_limit;
  }
  return GetSpeedLimitFromS(sl.s());
}

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


<commit_before>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "modules/video_capture/windows/video_capture_ds.h"

#include "modules/video_capture/video_capture_config.h"
#include "modules/video_capture/windows/help_functions_ds.h"
#include "modules/video_capture/windows/sink_filter_ds.h"
#include "rtc_base/logging.h"

#include <dvdmedia.h>  // VIDEOINFOHEADER2

namespace webrtc {
namespace videocapturemodule {
VideoCaptureDS::VideoCaptureDS()
    : _captureFilter(NULL),
      _graphBuilder(NULL),
      _mediaControl(NULL),
      _sinkFilter(NULL),
      _inputSendPin(NULL),
      _outputCapturePin(NULL),
      _dvFilter(NULL),
      _inputDvPin(NULL),
      _outputDvPin(NULL) {}

VideoCaptureDS::~VideoCaptureDS() {
  if (_mediaControl) {
    _mediaControl->Stop();
  }
  if (_graphBuilder) {
    if (_sinkFilter)
      _graphBuilder->RemoveFilter(_sinkFilter);
    if (_captureFilter)
      _graphBuilder->RemoveFilter(_captureFilter);
    if (_dvFilter)
      _graphBuilder->RemoveFilter(_dvFilter);
  }
  RELEASE_AND_CLEAR(_inputSendPin);
  RELEASE_AND_CLEAR(_outputCapturePin);

  RELEASE_AND_CLEAR(_captureFilter);  // release the capture device
  RELEASE_AND_CLEAR(_sinkFilter);
  RELEASE_AND_CLEAR(_dvFilter);

  RELEASE_AND_CLEAR(_mediaControl);

  RELEASE_AND_CLEAR(_inputDvPin);
  RELEASE_AND_CLEAR(_outputDvPin);

  RELEASE_AND_CLEAR(_graphBuilder);
}

int32_t VideoCaptureDS::Init(const char* deviceUniqueIdUTF8) {
  const int32_t nameLength = (int32_t)strlen((char*)deviceUniqueIdUTF8);
  if (nameLength > kVideoCaptureUniqueNameLength)
    return -1;

  // Store the device name
  _deviceUniqueId = new (std::nothrow) char[nameLength + 1];
  memcpy(_deviceUniqueId, deviceUniqueIdUTF8, nameLength + 1);

  if (_dsInfo.Init() != 0)
    return -1;

  _captureFilter = _dsInfo.GetDeviceFilter(deviceUniqueIdUTF8);
  if (!_captureFilter) {
    RTC_LOG(LS_INFO) << "Failed to create capture filter.";
    return -1;
  }

  // Get the interface for DirectShow's GraphBuilder
  HRESULT hr = CoCreateInstance(CLSID_FilterGraph, NULL, CLSCTX_INPROC_SERVER,
                                IID_IGraphBuilder, (void**)&_graphBuilder);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to create graph builder.";
    return -1;
  }

  hr = _graphBuilder->QueryInterface(IID_IMediaControl, (void**)&_mediaControl);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to create media control builder.";
    return -1;
  }
  hr = _graphBuilder->AddFilter(_captureFilter, CAPTURE_FILTER_NAME);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to add the capture device to the graph.";
    return -1;
  }

  _outputCapturePin = GetOutputPin(_captureFilter, PIN_CATEGORY_CAPTURE);

  // Create the sink filte used for receiving Captured frames.
  _sinkFilter = new CaptureSinkFilter(SINK_FILTER_NAME, NULL, &hr, *this);
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to create send filter";
    return -1;
  }
  _sinkFilter->AddRef();

  hr = _graphBuilder->AddFilter(_sinkFilter, SINK_FILTER_NAME);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to add the send filter to the graph.";
    return -1;
  }
  _inputSendPin = GetInputPin(_sinkFilter);

  // Temporary connect here.
  // This is done so that no one else can use the capture device.
  if (SetCameraOutput(_requestedCapability) != 0) {
    return -1;
  }
  hr = _mediaControl->Pause();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO)
        << "Failed to Pause the Capture device. Is it already occupied? " << hr;
    return -1;
  }
  RTC_LOG(LS_INFO) << "Capture device '" << deviceUniqueIdUTF8
                   << "' initialized.";
  return 0;
}

int32_t VideoCaptureDS::StartCapture(const VideoCaptureCapability& capability) {
  rtc::CritScope cs(&_apiCs);

  if (capability != _requestedCapability) {
    DisconnectGraph();

    if (SetCameraOutput(capability) != 0) {
      return -1;
    }
  }
  HRESULT hr = _mediaControl->Run();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to start the Capture device.";
    return -1;
  }
  return 0;
}

int32_t VideoCaptureDS::StopCapture() {
  rtc::CritScope cs(&_apiCs);

  HRESULT hr = _mediaControl->Pause();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to stop the capture graph. " << hr;
    return -1;
  }
  return 0;
}
bool VideoCaptureDS::CaptureStarted() {
  OAFilterState state = 0;
  HRESULT hr = _mediaControl->GetState(1000, &state);
  if (hr != S_OK && hr != VFW_S_CANT_CUE) {
    RTC_LOG(LS_INFO) << "Failed to get the CaptureStarted status";
  }
  RTC_LOG(LS_INFO) << "CaptureStarted " << state;
  return state == State_Running;
}
int32_t VideoCaptureDS::CaptureSettings(VideoCaptureCapability& settings) {
  settings = _requestedCapability;
  return 0;
}

int32_t VideoCaptureDS::SetCameraOutput(
    const VideoCaptureCapability& requestedCapability) {
  // Get the best matching capability
  VideoCaptureCapability capability;
  int32_t capabilityIndex;

  // Store the new requested size
  _requestedCapability = requestedCapability;
  // Match the requested capability with the supported.
  if ((capabilityIndex = _dsInfo.GetBestMatchedCapability(
           _deviceUniqueId, _requestedCapability, capability)) < 0) {
    return -1;
  }
  // Reduce the frame rate if possible.
  if (capability.maxFPS > requestedCapability.maxFPS) {
    capability.maxFPS = requestedCapability.maxFPS;
  } else if (capability.maxFPS <= 0) {
    capability.maxFPS = 30;
  }

  // Convert it to the windows capability index since they are not nexessary
  // the same
  VideoCaptureCapabilityWindows windowsCapability;
  if (_dsInfo.GetWindowsCapability(capabilityIndex, windowsCapability) != 0) {
    return -1;
  }

  IAMStreamConfig* streamConfig = NULL;
  AM_MEDIA_TYPE* pmt = NULL;
  VIDEO_STREAM_CONFIG_CAPS caps;

  HRESULT hr = _outputCapturePin->QueryInterface(IID_IAMStreamConfig,
                                                 (void**)&streamConfig);
  if (hr) {
    RTC_LOG(LS_INFO) << "Can't get the Capture format settings.";
    return -1;
  }

  // Get the windows capability from the capture device
  bool isDVCamera = false;
  hr = streamConfig->GetStreamCaps(windowsCapability.directShowCapabilityIndex,
                                   &pmt, reinterpret_cast<BYTE*>(&caps));
  if (!FAILED(hr)) {
    if (pmt->formattype == FORMAT_VideoInfo2) {
      VIDEOINFOHEADER2* h = reinterpret_cast<VIDEOINFOHEADER2*>(pmt->pbFormat);
      if (capability.maxFPS > 0 && windowsCapability.supportFrameRateControl) {
        h->AvgTimePerFrame = REFERENCE_TIME(10000000.0 / capability.maxFPS);
      }
    } else {
      VIDEOINFOHEADER* h = reinterpret_cast<VIDEOINFOHEADER*>(pmt->pbFormat);
      if (capability.maxFPS > 0 && windowsCapability.supportFrameRateControl) {
        h->AvgTimePerFrame = REFERENCE_TIME(10000000.0 / capability.maxFPS);
      }
    }

    // Set the sink filter to request this capability
    _sinkFilter->SetMatchingMediaType(capability);
    // Order the capture device to use this capability
    hr += streamConfig->SetFormat(pmt);

    // Check if this is a DV camera and we need to add MS DV Filter
    if (pmt->subtype == MEDIASUBTYPE_dvsl ||
        pmt->subtype == MEDIASUBTYPE_dvsd || pmt->subtype == MEDIASUBTYPE_dvhd)
      isDVCamera = true;  // This is a DV camera. Use MS DV filter
  }
  RELEASE_AND_CLEAR(streamConfig);

  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to set capture device output format";
    return -1;
  }

  if (isDVCamera) {
    hr = ConnectDVCamera();
  } else {
    hr = _graphBuilder->ConnectDirect(_outputCapturePin, _inputSendPin, NULL);
  }
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to connect the Capture graph " << hr;
    return -1;
  }
  return 0;
}

int32_t VideoCaptureDS::DisconnectGraph() {
  HRESULT hr = _mediaControl->Stop();
  hr += _graphBuilder->Disconnect(_outputCapturePin);
  hr += _graphBuilder->Disconnect(_inputSendPin);

  // if the DV camera filter exist
  if (_dvFilter) {
    _graphBuilder->Disconnect(_inputDvPin);
    _graphBuilder->Disconnect(_outputDvPin);
  }
  if (hr != S_OK) {
    RTC_LOG(LS_ERROR)
        << "Failed to Stop the Capture device for reconfiguration " << hr;
    return -1;
  }
  return 0;
}
HRESULT VideoCaptureDS::ConnectDVCamera() {
  HRESULT hr = S_OK;

  if (!_dvFilter) {
    hr = CoCreateInstance(CLSID_DVVideoCodec, NULL, CLSCTX_INPROC,
                          IID_IBaseFilter, (void**)&_dvFilter);
    if (hr != S_OK) {
      RTC_LOG(LS_INFO) << "Failed to create the dv decoder: " << hr;
      return hr;
    }
    hr = _graphBuilder->AddFilter(_dvFilter, L"VideoDecoderDV");
    if (hr != S_OK) {
      RTC_LOG(LS_INFO) << "Failed to add the dv decoder to the graph: " << hr;
      return hr;
    }
    _inputDvPin = GetInputPin(_dvFilter);
    if (_inputDvPin == NULL) {
      RTC_LOG(LS_INFO) << "Failed to get input pin from DV decoder";
      return -1;
    }
    _outputDvPin = GetOutputPin(_dvFilter, GUID_NULL);
    if (_outputDvPin == NULL) {
      RTC_LOG(LS_INFO) << "Failed to get output pin from DV decoder";
      return -1;
    }
  }
  hr = _graphBuilder->ConnectDirect(_outputCapturePin, _inputDvPin, NULL);
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to connect capture device to the dv devoder: "
                     << hr;
    return hr;
  }

  hr = _graphBuilder->ConnectDirect(_outputDvPin, _inputSendPin, NULL);
  if (hr != S_OK) {
    if (hr == HRESULT_FROM_WIN32(ERROR_TOO_MANY_OPEN_FILES)) {
      RTC_LOG(LS_INFO) << "Failed to connect the capture device, busy";
    } else {
      RTC_LOG(LS_INFO) << "Failed to connect capture device to the send graph: "
                       << hr;
    }
    return hr;
  }
  return hr;
}
}  // namespace videocapturemodule
}  // namespace webrtc
<commit_msg>Add guards to VideoCaptureDS::Init for when pins are null<commit_after>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "modules/video_capture/windows/video_capture_ds.h"

#include "modules/video_capture/video_capture_config.h"
#include "modules/video_capture/windows/help_functions_ds.h"
#include "modules/video_capture/windows/sink_filter_ds.h"
#include "rtc_base/logging.h"

#include <dvdmedia.h>  // VIDEOINFOHEADER2

namespace webrtc {
namespace videocapturemodule {
VideoCaptureDS::VideoCaptureDS()
    : _captureFilter(NULL),
      _graphBuilder(NULL),
      _mediaControl(NULL),
      _sinkFilter(NULL),
      _inputSendPin(NULL),
      _outputCapturePin(NULL),
      _dvFilter(NULL),
      _inputDvPin(NULL),
      _outputDvPin(NULL) {}

VideoCaptureDS::~VideoCaptureDS() {
  if (_mediaControl) {
    _mediaControl->Stop();
  }
  if (_graphBuilder) {
    if (_sinkFilter)
      _graphBuilder->RemoveFilter(_sinkFilter);
    if (_captureFilter)
      _graphBuilder->RemoveFilter(_captureFilter);
    if (_dvFilter)
      _graphBuilder->RemoveFilter(_dvFilter);
  }
  RELEASE_AND_CLEAR(_inputSendPin);
  RELEASE_AND_CLEAR(_outputCapturePin);

  RELEASE_AND_CLEAR(_captureFilter);  // release the capture device
  RELEASE_AND_CLEAR(_sinkFilter);
  RELEASE_AND_CLEAR(_dvFilter);

  RELEASE_AND_CLEAR(_mediaControl);

  RELEASE_AND_CLEAR(_inputDvPin);
  RELEASE_AND_CLEAR(_outputDvPin);

  RELEASE_AND_CLEAR(_graphBuilder);
}

int32_t VideoCaptureDS::Init(const char* deviceUniqueIdUTF8) {
  const int32_t nameLength = (int32_t)strlen((char*)deviceUniqueIdUTF8);
  if (nameLength > kVideoCaptureUniqueNameLength)
    return -1;

  // Store the device name
  _deviceUniqueId = new (std::nothrow) char[nameLength + 1];
  memcpy(_deviceUniqueId, deviceUniqueIdUTF8, nameLength + 1);

  if (_dsInfo.Init() != 0)
    return -1;

  _captureFilter = _dsInfo.GetDeviceFilter(deviceUniqueIdUTF8);
  if (!_captureFilter) {
    RTC_LOG(LS_INFO) << "Failed to create capture filter.";
    return -1;
  }

  // Get the interface for DirectShow's GraphBuilder
  HRESULT hr = CoCreateInstance(CLSID_FilterGraph, NULL, CLSCTX_INPROC_SERVER,
                                IID_IGraphBuilder, (void**)&_graphBuilder);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to create graph builder.";
    return -1;
  }

  hr = _graphBuilder->QueryInterface(IID_IMediaControl, (void**)&_mediaControl);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to create media control builder.";
    return -1;
  }
  hr = _graphBuilder->AddFilter(_captureFilter, CAPTURE_FILTER_NAME);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to add the capture device to the graph.";
    return -1;
  }

  _outputCapturePin = GetOutputPin(_captureFilter, PIN_CATEGORY_CAPTURE);
  if (!_outputCapturePin) {
    RTC_LOG(LS_INFO) << "Failed to get output capture pin";
    return -1;
  }

  // Create the sink filte used for receiving Captured frames.
  _sinkFilter = new CaptureSinkFilter(SINK_FILTER_NAME, NULL, &hr, *this);
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to create send filter";
    return -1;
  }
  _sinkFilter->AddRef();

  hr = _graphBuilder->AddFilter(_sinkFilter, SINK_FILTER_NAME);
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to add the send filter to the graph.";
    return -1;
  }

  _inputSendPin = GetInputPin(_sinkFilter);
  if (!_inputSendPin) {
    RTC_LOG(LS_INFO) << "Failed to get input send pin";
    return -1;
  }

  // Temporary connect here.
  // This is done so that no one else can use the capture device.
  if (SetCameraOutput(_requestedCapability) != 0) {
    return -1;
  }
  hr = _mediaControl->Pause();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO)
        << "Failed to Pause the Capture device. Is it already occupied? " << hr;
    return -1;
  }
  RTC_LOG(LS_INFO) << "Capture device '" << deviceUniqueIdUTF8
                   << "' initialized.";
  return 0;
}

int32_t VideoCaptureDS::StartCapture(const VideoCaptureCapability& capability) {
  rtc::CritScope cs(&_apiCs);

  if (capability != _requestedCapability) {
    DisconnectGraph();

    if (SetCameraOutput(capability) != 0) {
      return -1;
    }
  }
  HRESULT hr = _mediaControl->Run();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to start the Capture device.";
    return -1;
  }
  return 0;
}

int32_t VideoCaptureDS::StopCapture() {
  rtc::CritScope cs(&_apiCs);

  HRESULT hr = _mediaControl->Pause();
  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to stop the capture graph. " << hr;
    return -1;
  }
  return 0;
}
bool VideoCaptureDS::CaptureStarted() {
  OAFilterState state = 0;
  HRESULT hr = _mediaControl->GetState(1000, &state);
  if (hr != S_OK && hr != VFW_S_CANT_CUE) {
    RTC_LOG(LS_INFO) << "Failed to get the CaptureStarted status";
  }
  RTC_LOG(LS_INFO) << "CaptureStarted " << state;
  return state == State_Running;
}
int32_t VideoCaptureDS::CaptureSettings(VideoCaptureCapability& settings) {
  settings = _requestedCapability;
  return 0;
}

int32_t VideoCaptureDS::SetCameraOutput(
    const VideoCaptureCapability& requestedCapability) {
  // Get the best matching capability
  VideoCaptureCapability capability;
  int32_t capabilityIndex;

  // Store the new requested size
  _requestedCapability = requestedCapability;
  // Match the requested capability with the supported.
  if ((capabilityIndex = _dsInfo.GetBestMatchedCapability(
           _deviceUniqueId, _requestedCapability, capability)) < 0) {
    return -1;
  }
  // Reduce the frame rate if possible.
  if (capability.maxFPS > requestedCapability.maxFPS) {
    capability.maxFPS = requestedCapability.maxFPS;
  } else if (capability.maxFPS <= 0) {
    capability.maxFPS = 30;
  }

  // Convert it to the windows capability index since they are not nexessary
  // the same
  VideoCaptureCapabilityWindows windowsCapability;
  if (_dsInfo.GetWindowsCapability(capabilityIndex, windowsCapability) != 0) {
    return -1;
  }

  IAMStreamConfig* streamConfig = NULL;
  AM_MEDIA_TYPE* pmt = NULL;
  VIDEO_STREAM_CONFIG_CAPS caps;

  HRESULT hr = _outputCapturePin->QueryInterface(IID_IAMStreamConfig,
                                                 (void**)&streamConfig);
  if (hr) {
    RTC_LOG(LS_INFO) << "Can't get the Capture format settings.";
    return -1;
  }

  // Get the windows capability from the capture device
  bool isDVCamera = false;
  hr = streamConfig->GetStreamCaps(windowsCapability.directShowCapabilityIndex,
                                   &pmt, reinterpret_cast<BYTE*>(&caps));
  if (!FAILED(hr)) {
    if (pmt->formattype == FORMAT_VideoInfo2) {
      VIDEOINFOHEADER2* h = reinterpret_cast<VIDEOINFOHEADER2*>(pmt->pbFormat);
      if (capability.maxFPS > 0 && windowsCapability.supportFrameRateControl) {
        h->AvgTimePerFrame = REFERENCE_TIME(10000000.0 / capability.maxFPS);
      }
    } else {
      VIDEOINFOHEADER* h = reinterpret_cast<VIDEOINFOHEADER*>(pmt->pbFormat);
      if (capability.maxFPS > 0 && windowsCapability.supportFrameRateControl) {
        h->AvgTimePerFrame = REFERENCE_TIME(10000000.0 / capability.maxFPS);
      }
    }

    // Set the sink filter to request this capability
    _sinkFilter->SetMatchingMediaType(capability);
    // Order the capture device to use this capability
    hr += streamConfig->SetFormat(pmt);

    // Check if this is a DV camera and we need to add MS DV Filter
    if (pmt->subtype == MEDIASUBTYPE_dvsl ||
        pmt->subtype == MEDIASUBTYPE_dvsd || pmt->subtype == MEDIASUBTYPE_dvhd)
      isDVCamera = true;  // This is a DV camera. Use MS DV filter
  }
  RELEASE_AND_CLEAR(streamConfig);

  if (FAILED(hr)) {
    RTC_LOG(LS_INFO) << "Failed to set capture device output format";
    return -1;
  }

  if (isDVCamera) {
    hr = ConnectDVCamera();
  } else {
    hr = _graphBuilder->ConnectDirect(_outputCapturePin, _inputSendPin, NULL);
  }
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to connect the Capture graph " << hr;
    return -1;
  }
  return 0;
}

int32_t VideoCaptureDS::DisconnectGraph() {
  HRESULT hr = _mediaControl->Stop();
  hr += _graphBuilder->Disconnect(_outputCapturePin);
  hr += _graphBuilder->Disconnect(_inputSendPin);

  // if the DV camera filter exist
  if (_dvFilter) {
    _graphBuilder->Disconnect(_inputDvPin);
    _graphBuilder->Disconnect(_outputDvPin);
  }
  if (hr != S_OK) {
    RTC_LOG(LS_ERROR)
        << "Failed to Stop the Capture device for reconfiguration " << hr;
    return -1;
  }
  return 0;
}
HRESULT VideoCaptureDS::ConnectDVCamera() {
  HRESULT hr = S_OK;

  if (!_dvFilter) {
    hr = CoCreateInstance(CLSID_DVVideoCodec, NULL, CLSCTX_INPROC,
                          IID_IBaseFilter, (void**)&_dvFilter);
    if (hr != S_OK) {
      RTC_LOG(LS_INFO) << "Failed to create the dv decoder: " << hr;
      return hr;
    }
    hr = _graphBuilder->AddFilter(_dvFilter, L"VideoDecoderDV");
    if (hr != S_OK) {
      RTC_LOG(LS_INFO) << "Failed to add the dv decoder to the graph: " << hr;
      return hr;
    }
    _inputDvPin = GetInputPin(_dvFilter);
    if (_inputDvPin == NULL) {
      RTC_LOG(LS_INFO) << "Failed to get input pin from DV decoder";
      return -1;
    }
    _outputDvPin = GetOutputPin(_dvFilter, GUID_NULL);
    if (_outputDvPin == NULL) {
      RTC_LOG(LS_INFO) << "Failed to get output pin from DV decoder";
      return -1;
    }
  }
  hr = _graphBuilder->ConnectDirect(_outputCapturePin, _inputDvPin, NULL);
  if (hr != S_OK) {
    RTC_LOG(LS_INFO) << "Failed to connect capture device to the dv devoder: "
                     << hr;
    return hr;
  }

  hr = _graphBuilder->ConnectDirect(_outputDvPin, _inputSendPin, NULL);
  if (hr != S_OK) {
    if (hr == HRESULT_FROM_WIN32(ERROR_TOO_MANY_OPEN_FILES)) {
      RTC_LOG(LS_INFO) << "Failed to connect the capture device, busy";
    } else {
      RTC_LOG(LS_INFO) << "Failed to connect capture device to the send graph: "
                       << hr;
    }
    return hr;
  }
  return hr;
}
}  // namespace videocapturemodule
}  // namespace webrtc
<|endoftext|>


<commit_before>/********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2012, Max-Planck-Gesellschaft
 * Copyright (c) 2012-2015, Inria
 * 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 the copyright holder 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.
 *
 ********************************************************************/

/*
 * You must specify the S_FUNCTION_NAME as the name of your S-function
 * (i.e. replace sfuntmpl_basic with the name of your S-function).
 */

#define S_FUNCTION_NAME  rmsg_pub_String
#define S_FUNCTION_LEVEL 2

#define STRINGIFY(x) #x
#define TOSTRING(x) STRINGIFY(x)

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */
#include "simstruc.h"

#pragma push_macro("RT")
#undef RT

#include <ros/ros.h>

// Generic Publisher
#include <matlab_ros_bridge/GenericPublisher.hpp>

// Message
#include <std_msgs/String.h>

#pragma pop_macro("RT")

#include <matlab_ros_bridge/RosMatlabBrigdeDefines.hpp>

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
  /* Function: mdlCheckParameters =============================================
   * Abstract:
   *    Validate our parameters to verify:
   *     o The numerator must be of a lower order than the denominator.
   *     o The sample time must be a real positive nonzero value.
   */
  static void mdlCheckParameters(SimStruct *S)
  {

    // Tsim
	if (mxIsEmpty( ssGetSFcnParam(S,0)) ||
			mxIsSparse( ssGetSFcnParam(S,0)) ||
			mxIsComplex( ssGetSFcnParam(S,0)) ||
			mxIsLogical( ssGetSFcnParam(S,0)) ||
			!mxIsNumeric( ssGetSFcnParam(S,0)) ||
			!mxIsDouble( ssGetSFcnParam(S,0)) ||
			mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) {
		ssSetErrorStatus(S,"Simulation time must be a single double Value");
		return;
	}

	// Prefix Topic
	if (!mxIsChar( ssGetSFcnParam(S,1)) ) {
		ssSetErrorStatus(S,"Prefix value must be char array (string)");
		return;
	}

    // String length
    if (mxIsEmpty( ssGetSFcnParam(S,2)) ||
            mxIsSparse( ssGetSFcnParam(S,2)) ||
            mxIsComplex( ssGetSFcnParam(S,2)) ||
            mxIsLogical( ssGetSFcnParam(S,2)) ||
			!mxIsChar(ssGetSFcnParam(S,2)) || //ssGetDTypeIdFromMxArray(ssGetSFcnParam(S,2))!=SS_UINT32 ||
            mxGetNumberOfElements(ssGetSFcnParam(S,2)) != 1) {
        ssSetErrorStatus(S,"String length must be a char");
        return;
    }

  }
#endif /* MDL_CHECK_PARAMETERS */

/* Error handling
 * --------------
 *
 * You should use the following technique to report errors encountered within
 * an S-function:
 *
 *       ssSetErrorStatus(S,"Error encountered due to ...");
 *       return;
 *
 * Note that the 2nd argument to ssSetErrorStatus must be persistent memory.
 * It cannot be a local variable. For example the following will cause
 * unpredictable errors:
 *
 *      mdlOutputs()
 *      {
 *         char msg[256];         {ILLEGAL: to fix use "static char msg[256];"}
 *         sprintf(msg,"Error due to %s", string);
 *         ssSetErrorStatus(S,msg);
 *         return;
 *      }
 *
 * See matlabroot/simulink/src/sfuntmpl_doc.c for more details.
 */

/*====================*
 * S-function methods *
 *====================*/

//double Tsim;

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    /* See sfuntmpl_doc.c for more details on the macros below */

    ssSetNumSFcnParams(S, 3);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink. */
    }
#endif

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;

    //const uint32_T strLength = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2)));
    const mxChar strLength = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2)));
    ssSetInputPortMatrixDimensions(S, 0, 1, strLength); // string
    ssSetInputPortDataType(S, 0, SS_UINT8);

	for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) {
		/*direct input signal access*/
    	ssSetInputPortRequiredContiguous(S, i, true);
		/*
		 * Set direct feedthrough flag (1=yes, 0=no).
		 * A port has direct feedthrough if the input is used in either
		 * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
		 * See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
		 */
		ssSetInputPortDirectFeedThrough(S, i, 1);
	}

    if (!ssSetNumOutputPorts(S, 0)) return;

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 1); // strLength
    ssSetNumPWork(S, 1); // GenericPub
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}

/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */

static void mdlInitializeSampleTimes(SimStruct *S)
{
    real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0));
    ssSetSampleTime(S, 0, Tsim);                      //DISCRETE_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

}

#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  /* Function: mdlInitializeConditions ========================================
   * Abstract:
   *    In this function, you should initialize the continuous and discrete
   *    states for your S-function block.  The initial states are placed
   *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
   *    You can also perform any other initialization activities that your
   *    S-function may require. Note, this routine will be called at the
   *    start of simulation and if it is present in an enabled subsystem
   *    configured to reset states, it will be call when the enabled subsystem
   *    restarts execution to reset the states.
   */
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */

#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START)
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */

static void mdlStart(SimStruct *S)
{
    SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
    // init ROS if not yet done.
    initROS(S);

    void** vecPWork = ssGetPWork(S);

//    ssGetIWork(S)[0] = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2)));
    ssGetIWork(S)[0] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2)));

    ros::NodeHandle nodeHandle(ros::this_node::getName());

    // get Topic Strings
    size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
    char* prefix_topic = (char*)mxMalloc(prefix_buflen);
    mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen);

    //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
	GenericPublisher<std_msgs::String>* pub
		= new GenericPublisher<std_msgs::String>(nodeHandle, prefix_topic, 10);
	vecPWork[0] = pub;

    // free char array
    mxFree(prefix_topic);

  }
#endif /*  MDL_START */

/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{

    // get Objects
    void** vecPWork = ssGetPWork(S);
    const uint32_T strLength = ssGetIWork(S)[0];

    // get Pointers
    // accessing inputs
    uint8_T* txt = (uint8_T*)ssGetInputPortSignal(S,0);

    std_msgs::String msg;
    msg.data.resize(strLength);
    memcpy(&msg.data[0], (char*)txt, strLength*sizeof(uint8_T));
//    for (unsigned int i=0; i < strLength; ++i) {
//    	msg.data[i] = (char)txt[i];
//    }

	GenericPublisher<std_msgs::String>* pub
			= (GenericPublisher<std_msgs::String>*)vecPWork[0];

	pub->publish(msg);

}

#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  /* Function: mdlUpdate ======================================================
   * Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */

#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */

/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    // get Objects
    void** vecPWork = ssGetPWork(S);
    GenericPublisher<std_msgs::String>* pub = (GenericPublisher<std_msgs::String>*)vecPWork[0];
    delete pub;

    SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
}

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif
<commit_msg>fixed char data type<commit_after>/********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2012, Max-Planck-Gesellschaft
 * Copyright (c) 2012-2015, Inria
 * 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 the copyright holder 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.
 *
 ********************************************************************/

/*
 * You must specify the S_FUNCTION_NAME as the name of your S-function
 * (i.e. replace sfuntmpl_basic with the name of your S-function).
 */

#define S_FUNCTION_NAME  rmsg_pub_String
#define S_FUNCTION_LEVEL 2

#define STRINGIFY(x) #x
#define TOSTRING(x) STRINGIFY(x)

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */
#include "simstruc.h"

#pragma push_macro("RT")
#undef RT

#include <ros/ros.h>

// Generic Publisher
#include <matlab_ros_bridge/GenericPublisher.hpp>

// Message
#include <std_msgs/String.h>

#pragma pop_macro("RT")

#include <matlab_ros_bridge/RosMatlabBrigdeDefines.hpp>

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
  /* Function: mdlCheckParameters =============================================
   * Abstract:
   *    Validate our parameters to verify:
   *     o The numerator must be of a lower order than the denominator.
   *     o The sample time must be a real positive nonzero value.
   */
  static void mdlCheckParameters(SimStruct *S)
  {

    // Tsim
	if (mxIsEmpty( ssGetSFcnParam(S,0)) ||
			mxIsSparse( ssGetSFcnParam(S,0)) ||
			mxIsComplex( ssGetSFcnParam(S,0)) ||
			mxIsLogical( ssGetSFcnParam(S,0)) ||
			!mxIsNumeric( ssGetSFcnParam(S,0)) ||
			!mxIsDouble( ssGetSFcnParam(S,0)) ||
			mxGetNumberOfElements(ssGetSFcnParam(S,0)) != 1) {
		ssSetErrorStatus(S,"Simulation time must be a single double Value");
		return;
	}

	// Prefix Topic
	if (!mxIsChar( ssGetSFcnParam(S,1)) ) {
		ssSetErrorStatus(S,"Prefix value must be char array (string)");
		return;
	}

    // String length
    if (mxIsEmpty( ssGetSFcnParam(S,2)) ||
            mxIsSparse( ssGetSFcnParam(S,2)) ||
            mxIsComplex( ssGetSFcnParam(S,2)) ||
            mxIsLogical( ssGetSFcnParam(S,2)) ||
			!mxIsChar(ssGetSFcnParam(S,2)) || //ssGetDTypeIdFromMxArray(ssGetSFcnParam(S,2))!=SS_UINT32 ||
            mxGetNumberOfElements(ssGetSFcnParam(S,2)) != 1) {
        ssSetErrorStatus(S,"String length must be a char");
        return;
    }

  }
#endif /* MDL_CHECK_PARAMETERS */

/* Error handling
 * --------------
 *
 * You should use the following technique to report errors encountered within
 * an S-function:
 *
 *       ssSetErrorStatus(S,"Error encountered due to ...");
 *       return;
 *
 * Note that the 2nd argument to ssSetErrorStatus must be persistent memory.
 * It cannot be a local variable. For example the following will cause
 * unpredictable errors:
 *
 *      mdlOutputs()
 *      {
 *         char msg[256];         {ILLEGAL: to fix use "static char msg[256];"}
 *         sprintf(msg,"Error due to %s", string);
 *         ssSetErrorStatus(S,msg);
 *         return;
 *      }
 *
 * See matlabroot/simulink/src/sfuntmpl_doc.c for more details.
 */

/*====================*
 * S-function methods *
 *====================*/

//double Tsim;

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    /* See sfuntmpl_doc.c for more details on the macros below */

    ssSetNumSFcnParams(S, 3);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink. */
    }
#endif

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;

    //const uint32_T strLength = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2)));
    const mxChar strLength = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2)));
    ssSetInputPortMatrixDimensions(S, 0, 1, strLength); // string
    ssSetInputPortDataType(S, 0, SS_UINT8);

	for (int_T i = 0; i < ssGetNumInputPorts(S); ++i) {
		/*direct input signal access*/
    	ssSetInputPortRequiredContiguous(S, i, true);
		/*
		 * Set direct feedthrough flag (1=yes, 0=no).
		 * A port has direct feedthrough if the input is used in either
		 * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
		 * See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
		 */
		ssSetInputPortDirectFeedThrough(S, i, 1);
	}

    if (!ssSetNumOutputPorts(S, 0)) return;

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 1); // strLength
    ssSetNumPWork(S, 1); // GenericPub
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}

/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */

static void mdlInitializeSampleTimes(SimStruct *S)
{
    real_T Tsim = mxGetScalar(ssGetSFcnParam(S, 0));
    ssSetSampleTime(S, 0, Tsim);                      //DISCRETE_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

}

#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  /* Function: mdlInitializeConditions ========================================
   * Abstract:
   *    In this function, you should initialize the continuous and discrete
   *    states for your S-function block.  The initial states are placed
   *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
   *    You can also perform any other initialization activities that your
   *    S-function may require. Note, this routine will be called at the
   *    start of simulation and if it is present in an enabled subsystem
   *    configured to reset states, it will be call when the enabled subsystem
   *    restarts execution to reset the states.
   */
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */

#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START)
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */

static void mdlStart(SimStruct *S)
{
    SFUNPRINTF("Creating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
    // init ROS if not yet done.
    initROS(S);

    void** vecPWork = ssGetPWork(S);

//    ssGetIWork(S)[0] = *((uint32_T*)mxGetData(ssGetSFcnParam(S, 2)));
    ssGetIWork(S)[0] = *((mxChar*)mxGetData(ssGetSFcnParam(S, 2)));

    ros::NodeHandle nodeHandle(ros::this_node::getName());

    // get Topic Strings
    size_t prefix_buflen = mxGetN((ssGetSFcnParam(S, 1)))*sizeof(mxChar)+1;
    char* prefix_topic = (char*)mxMalloc(prefix_buflen);
    mxGetString((ssGetSFcnParam(S, 1)), prefix_topic, prefix_buflen);

    //SFUNPRINTF("The string being passed as a Paramater is - %s\n ", topic);
	GenericPublisher<std_msgs::String>* pub
		= new GenericPublisher<std_msgs::String>(nodeHandle, prefix_topic, 10);
	vecPWork[0] = pub;

    // free char array
    mxFree(prefix_topic);

  }
#endif /*  MDL_START */

/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{

    // get Objects
    void** vecPWork = ssGetPWork(S);
    //const uint32_T strLength = ssGetIWork(S)[0];
    const mxChar strLength = ssGetIWork(S)[0];

    // get Pointers
    // accessing inputs
    uint8_T* txt = (uint8_T*)ssGetInputPortSignal(S,0);

    std_msgs::String msg;
    msg.data.resize(strLength);
    memcpy(&msg.data[0], (char*)txt, strLength*sizeof(char));
//    for (unsigned int i=0; i < strLength; ++i) {
//    	msg.data[i] = (char)txt[i];
//    }

	GenericPublisher<std_msgs::String>* pub
			= (GenericPublisher<std_msgs::String>*)vecPWork[0];

	pub->publish(msg);

}

#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  /* Function: mdlUpdate ======================================================
   * Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */

#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */

/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
    // get Objects
    void** vecPWork = ssGetPWork(S);
    GenericPublisher<std_msgs::String>* pub = (GenericPublisher<std_msgs::String>*)vecPWork[0];
    delete pub;

    SFUNPRINTF("Terminating Instance of %s.\n", TOSTRING(S_FUNCTION_NAME));
}

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif
<|endoftext|>


<commit_before>#pragma GCC optimize ("-O3")

#include "libgame.h"
#include "storage.h"

#define _OUTPUT_INSTANCES
#include "games.h"
#undef _OUTPUT_INSTANCES

#define UP BITMASK(BUTTON_NE) | BITMASK(BUTTON_UP)
#define DOWN BITMASK(BUTTON_SE) | BITMASK(BUTTON_DOWN)
#define SELECT BITMASK(BUTTON_SW) | BITMASK(BUTTON_START)

// need some space for stack and system variables
#define AVAIL_SPACE 1024

static uint8_t N_GAMES;

static uint8_t sel = 0;

static uint8_t memory[AVAIL_SPACE];

static unsigned long cur_time = 0;

static unsigned long btn_timeout = 0;
#define BUTTON_DELAY 200

static bool game_running = false;
static bool btn_pressed = false;
static game_instance* ptr;
void prepare()
{
    N_GAMES = sizeof(instances) / sizeof(game_instance);
    game_setup();
    storage_init();
    game_set_ups(60);
}

void update(unsigned long delta)
{
    cur_time += delta;
    if (!game_running)
    {
        if (cur_time < btn_timeout) return;
        if ((game_is_any_button_pressed(DOWN)) && sel < (N_GAMES - 1))
        {
            sel++;
            btn_timeout = cur_time + BUTTON_DELAY;
        }
        if ((game_is_any_button_pressed(UP)) && sel > 0)
        {
            sel--;
            btn_timeout = cur_time + BUTTON_DELAY;
        }
        if (game_is_any_button_pressed(SELECT)) btn_pressed = true;
        if (!game_is_any_button_pressed(SELECT) && btn_pressed)
        {
            // run game
            ptr = instances + sel;
            game_running = true;
            *(ptr->data) = memory;
            ptr->prepare();
        }
    }
    else
    {
        ptr->update(delta);
    }
}

void render()
{
    if (!game_running)
    {
        for (uint8_t iter = 0; iter < N_GAMES; ++iter)
        {
            if (instances[iter].name)
            {
                game_draw_text(instances[iter].name, 0, 8 * iter, (iter == sel) ? RED : WHITE);
            }
        }
    }
    else
    {
        ptr->render();
    }
}

//////////////////////////
// Storage
//////////////////////////

void game_save(const void *buf, uint16_t size)
{
    storage_write((const char*)ptr->name, buf, size);
}

void game_load(void *buf, uint16_t size)
{
    storage_read((const char*)ptr->name, buf, size);
}

<commit_msg>Removed useless variables<commit_after>#pragma GCC optimize ("-O3")

#include "libgame.h"
#include "storage.h"

#define _OUTPUT_INSTANCES
#include "games.h"
#undef _OUTPUT_INSTANCES

#define UP BITMASK(BUTTON_NE) | BITMASK(BUTTON_UP)
#define DOWN BITMASK(BUTTON_SE) | BITMASK(BUTTON_DOWN)
#define SELECT BITMASK(BUTTON_SW) | BITMASK(BUTTON_START)

// need some space for stack and system variables
#define AVAIL_SPACE 1024

#define N_GAMES (sizeof(instances) / sizeof(game_instance))

static uint8_t sel = 0;

static uint8_t memory[AVAIL_SPACE];

static unsigned long cur_time = 0;

static unsigned long btn_timeout = 0;
#define BUTTON_DELAY 200

static bool btn_pressed = false;
static game_instance* ptr;
void prepare()
{
    game_setup();
    storage_init();
    game_set_ups(60);
}

void update(unsigned long delta)
{
    cur_time += delta;
    if (!ptr)
    {
        if (cur_time < btn_timeout) return;
        if ((game_is_any_button_pressed(DOWN)) && sel < (N_GAMES - 1))
        {
            sel++;
            btn_timeout = cur_time + BUTTON_DELAY;
        }
        if ((game_is_any_button_pressed(UP)) && sel > 0)
        {
            sel--;
            btn_timeout = cur_time + BUTTON_DELAY;
        }
        if (game_is_any_button_pressed(SELECT)) btn_pressed = true;
        if (!game_is_any_button_pressed(SELECT) && btn_pressed)
        {
            // run game
            ptr = instances + sel;
            *(ptr->data) = memory;
            ptr->prepare();
        }
    }
    else
    {
        ptr->update(delta);
    }
}

void render()
{
    if (!ptr)
    {
        for (uint8_t iter = 0; iter < N_GAMES; ++iter)
        {
            if (instances[iter].name)
            {
                game_draw_text(instances[iter].name, 0, 8 * iter, (iter == sel) ? RED : WHITE);
            }
        }
    }
    else
    {
        ptr->render();
    }
}

//////////////////////////
// Storage
//////////////////////////

void game_save(const void *buf, uint16_t size)
{
    storage_write((const char*)ptr->name, buf, size);
}

void game_load(void *buf, uint16_t size)
{
    storage_read((const char*)ptr->name, buf, size);
}

<|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright 2009-2013 Jörg Müller
 *
 * 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.
 ******************************************************************************/

#include "SequenceHandle.h"
#include "sequence/SequenceEntry.h"
#include "devices/ReadDevice.h"

#include <mutex>

#define KEEP_TIME 10

AUD_NAMESPACE_BEGIN

void SequenceHandle::start()
{
	// we already tried to start, aborting
	if(!m_valid)
		return;

	// in case the sound is playing, we need to stop first
	stop();

	std::lock_guard<ILockable> lock(*m_entry);

	// let's try playing
	if(m_entry->m_sound.get())
	{
		m_handle = m_device.play(m_entry->m_sound, true);
		m_3dhandle = std::dynamic_pointer_cast<I3DHandle>(m_handle);

		// after starting we have to set the properties, so let's ensure that
		m_status--;
	}

	// if the sound could not be played, we invalidate
	m_valid = m_handle.get();
}

bool SequenceHandle::updatePosition(float position)
{
	std::lock_guard<ILockable> lock(*m_entry);

	if(m_handle.get())
	{
		// we currently have a handle, let's check where we are
		if(position >= m_entry->m_end)
		{
			if(position >= m_entry->m_end + KEEP_TIME)
				// far end, stopping
				stop();
			else
			{
				// close end, just pausing
				m_handle->pause();
				return true;
			}
		}
		else if(position >= m_entry->m_begin)
		{
			// inside, resuming
			m_handle->resume();
			return true;
		}
		else
		{
			if(position < m_entry->m_begin - KEEP_TIME)
				// far beginning, stopping
				stop();
			else
			{
				// close beginning, just pausing
				m_handle->pause();
				return true;
			}
		}
	}
	else
	{
		// we don't have a handle, let's start if we should be playing
		if(position >= m_entry->m_begin && position <= m_entry->m_end)
		{
			start();
			return m_valid;
		}
	}

	return false;
}

SequenceHandle::SequenceHandle(std::shared_ptr<SequenceEntry> entry, ReadDevice& device) :
	m_entry(entry),
	m_valid(true),
	m_status(0),
	m_pos_status(0),
	m_sound_status(0),
	m_device(device)
{
}

SequenceHandle::~SequenceHandle()
{
	stop();
}

int SequenceHandle::compare(std::shared_ptr<SequenceEntry> entry) const
{
	if(m_entry->getID() < entry->getID())
		return -1;
	else if(m_entry->getID() == entry->getID())
		return 0;
	return 1;
}

void SequenceHandle::stop()
{
	if(m_handle.get())
		m_handle->stop();
	m_handle = nullptr;
	m_3dhandle = nullptr;
}

void SequenceHandle::update(float position, float frame, float fps)
{
	if(m_sound_status != m_entry->m_sound_status)
	{
		// if a new sound has been set, it's possible to get valid again!
		m_sound_status = m_entry->m_sound_status;
		m_valid = true;

		// stop whatever sound has been playing
		stop();

		// seek starts and seeks to the correct position
		if(!seek(position))
			// no handle, aborting
			return;
	}
	else
	{
		if(!m_valid)
			// invalid, aborting
			return;

		if(m_handle.get())
		{
			// we have a handle, let's update the position
			if(!updatePosition(position))
				// lost handle, aborting
				return;
		}
		else
		{
			// we don't have a handle, let's see if we can start
			if(!seek(position))
				return;
		}
	}

	std::lock_guard<ILockable> lock(*m_entry);
	if(m_pos_status != m_entry->m_pos_status)
	{
		m_pos_status = m_entry->m_pos_status;

		// position changed, need to seek
		if(!seek(position))
			// lost handle, aborting
			return;
	}

	// so far everything alright and handle is there, let's keep going

	if(m_status != m_entry->m_status)
	{
		m_3dhandle->setRelative(m_entry->m_relative);
		m_3dhandle->setVolumeMaximum(m_entry->m_volume_max);
		m_3dhandle->setVolumeMinimum(m_entry->m_volume_min);
		m_3dhandle->setDistanceMaximum(m_entry->m_distance_max);
		m_3dhandle->setDistanceReference(m_entry->m_distance_reference);
		m_3dhandle->setAttenuation(m_entry->m_attenuation);
		m_3dhandle->setConeAngleOuter(m_entry->m_cone_angle_outer);
		m_3dhandle->setConeAngleInner(m_entry->m_cone_angle_inner);
		m_3dhandle->setConeVolumeOuter(m_entry->m_cone_volume_outer);

		m_status = m_entry->m_status;
	}

	float value;

	m_entry->m_volume.read(frame, &value);
	m_handle->setVolume(value);
	m_entry->m_pitch.read(frame, &value);
	m_handle->setPitch(value);
	m_entry->m_panning.read(frame, &value);
	SoftwareDevice::setPanning(m_handle.get(), value);

	Vector3 v, v2;
	Quaternion q;

	m_entry->m_orientation.read(frame, q.get());
	m_3dhandle->setOrientation(q);
	m_entry->m_location.read(frame, v.get());
	m_3dhandle->setLocation(v);
	m_entry->m_location.read(frame + 1, v2.get());
	v2 -= v;
	m_3dhandle->setVelocity(v2 * fps);

	if(m_entry->m_muted)
		m_handle->setVolume(0);
}

bool SequenceHandle::seek(float position)
{
	if(!m_valid)
		// sound not valid, aborting
		return false;

	if(!updatePosition(position))
		// no handle, aborting
		return false;

	std::lock_guard<ILockable> lock(*m_entry);
	float seekpos = position - m_entry->m_begin;
	if(seekpos < 0)
		seekpos = 0;
	seekpos += m_entry->m_skip;
	m_handle->setPitch(1.0f);
	m_handle->seek(seekpos);

	return true;
}

AUD_NAMESPACE_END
<commit_msg>Catching exceptions in SequenceHandle when it tries to play a sound.<commit_after>/*******************************************************************************
 * Copyright 2009-2013 Jörg Müller
 *
 * 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.
 ******************************************************************************/

#include "SequenceHandle.h"
#include "sequence/SequenceEntry.h"
#include "devices/ReadDevice.h"

#include <mutex>

#define KEEP_TIME 10

AUD_NAMESPACE_BEGIN

void SequenceHandle::start()
{
	// we already tried to start, aborting
	if(!m_valid)
		return;

	// in case the sound is playing, we need to stop first
	stop();

	std::lock_guard<ILockable> lock(*m_entry);

	// let's try playing
	if(m_entry->m_sound.get())
	{
		try
		{
			m_handle = m_device.play(m_entry->m_sound, true);
			m_3dhandle = std::dynamic_pointer_cast<I3DHandle>(m_handle);
		}
		catch(AUD_Exception&)
		{
			// handle stays invalid in case we get an exception
		}

		// after starting we have to set the properties, so let's ensure that
		m_status--;
	}

	// if the sound could not be played, we invalidate
	m_valid = m_handle.get();
}

bool SequenceHandle::updatePosition(float position)
{
	std::lock_guard<ILockable> lock(*m_entry);

	if(m_handle.get())
	{
		// we currently have a handle, let's check where we are
		if(position >= m_entry->m_end)
		{
			if(position >= m_entry->m_end + KEEP_TIME)
				// far end, stopping
				stop();
			else
			{
				// close end, just pausing
				m_handle->pause();
				return true;
			}
		}
		else if(position >= m_entry->m_begin)
		{
			// inside, resuming
			m_handle->resume();
			return true;
		}
		else
		{
			if(position < m_entry->m_begin - KEEP_TIME)
				// far beginning, stopping
				stop();
			else
			{
				// close beginning, just pausing
				m_handle->pause();
				return true;
			}
		}
	}
	else
	{
		// we don't have a handle, let's start if we should be playing
		if(position >= m_entry->m_begin && position <= m_entry->m_end)
		{
			start();
			return m_valid;
		}
	}

	return false;
}

SequenceHandle::SequenceHandle(std::shared_ptr<SequenceEntry> entry, ReadDevice& device) :
	m_entry(entry),
	m_valid(true),
	m_status(0),
	m_pos_status(0),
	m_sound_status(0),
	m_device(device)
{
}

SequenceHandle::~SequenceHandle()
{
	stop();
}

int SequenceHandle::compare(std::shared_ptr<SequenceEntry> entry) const
{
	if(m_entry->getID() < entry->getID())
		return -1;
	else if(m_entry->getID() == entry->getID())
		return 0;
	return 1;
}

void SequenceHandle::stop()
{
	if(m_handle.get())
		m_handle->stop();
	m_handle = nullptr;
	m_3dhandle = nullptr;
}

void SequenceHandle::update(float position, float frame, float fps)
{
	if(m_sound_status != m_entry->m_sound_status)
	{
		// if a new sound has been set, it's possible to get valid again!
		m_sound_status = m_entry->m_sound_status;
		m_valid = true;

		// stop whatever sound has been playing
		stop();

		// seek starts and seeks to the correct position
		if(!seek(position))
			// no handle, aborting
			return;
	}
	else
	{
		if(!m_valid)
			// invalid, aborting
			return;

		if(m_handle.get())
		{
			// we have a handle, let's update the position
			if(!updatePosition(position))
				// lost handle, aborting
				return;
		}
		else
		{
			// we don't have a handle, let's see if we can start
			if(!seek(position))
				return;
		}
	}

	std::lock_guard<ILockable> lock(*m_entry);
	if(m_pos_status != m_entry->m_pos_status)
	{
		m_pos_status = m_entry->m_pos_status;

		// position changed, need to seek
		if(!seek(position))
			// lost handle, aborting
			return;
	}

	// so far everything alright and handle is there, let's keep going

	if(m_status != m_entry->m_status)
	{
		m_3dhandle->setRelative(m_entry->m_relative);
		m_3dhandle->setVolumeMaximum(m_entry->m_volume_max);
		m_3dhandle->setVolumeMinimum(m_entry->m_volume_min);
		m_3dhandle->setDistanceMaximum(m_entry->m_distance_max);
		m_3dhandle->setDistanceReference(m_entry->m_distance_reference);
		m_3dhandle->setAttenuation(m_entry->m_attenuation);
		m_3dhandle->setConeAngleOuter(m_entry->m_cone_angle_outer);
		m_3dhandle->setConeAngleInner(m_entry->m_cone_angle_inner);
		m_3dhandle->setConeVolumeOuter(m_entry->m_cone_volume_outer);

		m_status = m_entry->m_status;
	}

	float value;

	m_entry->m_volume.read(frame, &value);
	m_handle->setVolume(value);
	m_entry->m_pitch.read(frame, &value);
	m_handle->setPitch(value);
	m_entry->m_panning.read(frame, &value);
	SoftwareDevice::setPanning(m_handle.get(), value);

	Vector3 v, v2;
	Quaternion q;

	m_entry->m_orientation.read(frame, q.get());
	m_3dhandle->setOrientation(q);
	m_entry->m_location.read(frame, v.get());
	m_3dhandle->setLocation(v);
	m_entry->m_location.read(frame + 1, v2.get());
	v2 -= v;
	m_3dhandle->setVelocity(v2 * fps);

	if(m_entry->m_muted)
		m_handle->setVolume(0);
}

bool SequenceHandle::seek(float position)
{
	if(!m_valid)
		// sound not valid, aborting
		return false;

	if(!updatePosition(position))
		// no handle, aborting
		return false;

	std::lock_guard<ILockable> lock(*m_entry);
	float seekpos = position - m_entry->m_begin;
	if(seekpos < 0)
		seekpos = 0;
	seekpos += m_entry->m_skip;
	m_handle->setPitch(1.0f);
	m_handle->seek(seekpos);

	return true;
}

AUD_NAMESPACE_END
<|endoftext|>