X-lanni
71 lines
2.2 KiB
C++
71 lines
2.2 KiB
C++
// Copyright (c) 2022 Samsung R&D Institute Russia
|
|
//
|
|
// 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 "nav2_collision_monitor/kinematics.hpp"
|
|
|
|
#include <cmath>
|
|
|
|
namespace nav2_collision_monitor
|
|
{
|
|
|
|
void transformPoints(const Pose & pose, std::vector<Point> & points)
|
|
{
|
|
const double cos_theta = std::cos(pose.theta);
|
|
const double sin_theta = std::sin(pose.theta);
|
|
|
|
for (Point & point : points) {
|
|
// p = R*p' + pose
|
|
// p' = Rt * (p - pose)
|
|
// where:
|
|
// p - point coordinates in initial frame
|
|
// p' - point coordinates in a new frame
|
|
// R - rotation matrix =
|
|
// [cos_theta -sin_theta]
|
|
// [sin_theta cos_theta]
|
|
// Rt - transposed (inverted) rotation matrix
|
|
const double mul_x = point.x - pose.x;
|
|
const double mul_y = point.y - pose.y;
|
|
point.x = mul_x * cos_theta + mul_y * sin_theta;
|
|
point.y = -mul_x * sin_theta + mul_y * cos_theta;
|
|
}
|
|
}
|
|
|
|
void projectState(const double & dt, Pose & pose, Velocity & velocity)
|
|
{
|
|
const double theta = velocity.tw * dt;
|
|
const double cos_theta = std::cos(theta);
|
|
const double sin_theta = std::sin(theta);
|
|
|
|
// p' = p + vel*dt
|
|
// where:
|
|
// p - initial pose
|
|
// p' - projected pose
|
|
pose.x = pose.x + velocity.x * dt;
|
|
pose.y = pose.y + velocity.y * dt;
|
|
// Rotate the pose on theta
|
|
pose.theta = pose.theta + theta;
|
|
|
|
// vel' = R*vel
|
|
// where:
|
|
// vel - initial velocity
|
|
// R - rotation matrix
|
|
// vel' - rotated velocity
|
|
const double velocity_upd_x = velocity.x * cos_theta - velocity.y * sin_theta;
|
|
const double velocity_upd_y = velocity.x * sin_theta + velocity.y * cos_theta;
|
|
velocity.x = velocity_upd_x;
|
|
velocity.y = velocity_upd_y;
|
|
}
|
|
|
|
} // namespace nav2_collision_monitor
|