ros_conversions.cpp
Go to the documentation of this file.
1 #include "ros_conversions.h"
2
3 #include <cmath>
4
8
12 #include <range/v3/view/drop.hpp>
13 #include <range/v3/view/zip.hpp>
14
16 {
17  Eigen::Vector2d
18  toRos2D(const Eigen::Vector2f& vec)
19  {
20  return vec.cast<double>() / 1000; // [mm] to [m]
21  }
22
23  Eigen::Vector2d
24  toRos(const Eigen::Vector3f& vec)
25  {
26  return conv::to2D(vec).cast<double>() / 1000; // [mm] to [m]
27  }
28
29  Eigen::Vector3f
30  fromRos(const Eigen::Vector2d& vec)
31  {
32  return conv::to3D(vec.cast<float>()) * 1000; // [m] to [mm]
33  }
34
35
36  teb_local_planner::PoseSE2
37  toRos(const core::Pose& pose)
38  {
39  core::Pose2D pose2D = to2D(pose);
41  }
42
43  teb_local_planner::PoseSE2
44  toRos(const core::Pose2D& pose)
45  {
46  Eigen::Vector2d pos = pose.translation().cast<double>();
47  double theta = Eigen::Rotation2Df(pose.linear()).angle();
48
49  // we change it such that x is pointing forward.
50  // ROS: x pointing forward, ArmarX: y pointing forward
51  theta += M_PI_2;
52  // normalize angle
53  theta = g2o::normalize_theta(theta);
54
55  return {pos / 1000., theta}; // [mm] to [m]
56  }
57
58
60  fromRos(const teb_local_planner::PoseSE2& pose)
61  {
62  Eigen::Vector2d pos = pose.position() * 1000; // [m] to [mm]
63  double theta = pose.theta();
64
65  // we change it such that x is pointing forward.
66  // ROS: x pointing forward, ArmarX: y pointing forward
67  theta -= M_PI_2;
68
69
71  ret.translation() = pos.cast<float>();
72  ret.linear() = Eigen::Rotation2Df(theta).toRotationMatrix();
73
75  }
76
77  geometry_msgs::Twist
78  toRos(const core::Twist& velocity)
79  {
80  geometry_msgs::Twist ret;
81
82  // we change it such that x is pointing forward.
83  // ROS: x pointing forward, ArmarX: y pointing forward
84
85  ret.linear.x = velocity.linear.y() / 1000; // [mm] to [m]
86  ret.linear.y = -velocity.linear.x() / 1000; // [mm] to [m]
87  ret.angular.z = velocity.angular.z();
88
89  return ret;
90  }
91
92  teb_local_planner::TimedElasticBand
93  toRos(const core::GlobalTrajectory& trajectory)
94  {
95  teb_local_planner::TimedElasticBand teb;
96
97  bool first = true;
98  teb_local_planner::PoseSE2 lastPose;
99  for (const core::GlobalTrajectoryPoint& point : trajectory.points())
100  {
101  teb_local_planner::PoseSE2 pose = toRos(point.waypoint.pose);
103
104  if (!first)
105  {
106  Eigen::Vector2d distance = pose.position() - lastPose.position();
107  double timeDiff = distance.norm() / (point.velocity / 1000); // [mm/s] -> [m/s]
109
110  first = false;
111  }
112
113  lastPose = pose;
114  }
115
116  return teb;
117  }
118
120  fromRos(const teb_local_planner::TimedElasticBand& teb)
121  {
122  const auto& tebPoses = teb.poses();
123
124  if (tebPoses.empty())
125  {
126  return {};
127  }
128
129  core::LocalTrajectoryPoints trajectory;
130  trajectory.reserve(tebPoses.size());
131
132  // TODO this timestamp should be given via the argument list
133  DateTime timestamp = Clock::Now();
134
135  ARMARX_CHECK_EQUAL(tebPoses.size() - 1, teb.timediffs().size());
136
137  // add the first pose at this timestamp
138  trajectory.push_back(core::LocalTrajectoryPoint{
139  .timestamp = timestamp, .pose = {fromRos(tebPoses.front()->pose())}});
140
141  // all consecutive poses
142  for (const auto& [poseVertex, timediff] :
143  ranges::views::zip(tebPoses | ranges::views::drop(1), teb.timediffs()))
144  {
145  const core::Pose pose = fromRos(poseVertex->pose());
146  const Duration dt = Duration::SecondsDouble(timediff->dt());
147
148  timestamp += dt;
149  trajectory.push_back(
150  core::LocalTrajectoryPoint{.timestamp = timestamp, .pose = {pose}});
151  }
152
153  ARMARX_CHECK_EQUAL(trajectory.size(), tebPoses.size());
154
155  return {trajectory};
156  }
157
160  {
161  // a,b parameter are along the x and y axis respectively
162  // ROS and ArmarX have the x axis pointing in a different direction
163  // ROS: x pointing forward, ArmarX: y pointing forward
164  return {.a = ellipse.b / 1000, .b = ellipse.a / 1000}; // [mm] to [m]
165  }
166
167  teb_local_planner::Costmap
168  toRos(const algorithms::Costmap& costmap)
169  {
170  const algorithms::Costmap::Parameters& params = costmap.params();
171  const teb_local_planner::Costmap::Parameters teb_params = {
172  params.binaryGrid, params.cellSize / 1000}; // [mm] to [m]
173
174  const algorithms::SceneBounds& bounds = costmap.getLocalSceneBounds();
175  const teb_local_planner::Costmap::SceneBounds teb_bounds = {
176  toRos2D(bounds.min).cast<float>(), toRos2D(bounds.max).cast<float>()};
177
181  {
183  }
184
185  teb_local_planner::Costmap::Pose2D teb_origin = costmap.origin();
186  teb_origin.translation() /= 1000; // [mm] to[m]
187
188  algorithms::Costmap::Grid teb_grid = costmap.getGrid();
189  teb_grid /= 1000; // [mm] to [m]
190
191  return teb_local_planner::Costmap{teb_grid, teb_params, teb_bounds, teb_mask, teb_origin};
192  }
193
194  teb_local_planner::PenaltyModelPtr
196  {
197  return boost::make_shared<teb_local_planner::LinearPenaltyModel>(
198  teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon));
199  }
200
201  teb_local_planner::PenaltyModelPtr
203  {
204  return boost::make_shared<teb_local_planner::ExponentialPenaltyModel>(
205  teb_local_planner::LinearPenaltyModel(model.minDistance, model.epsilon),
206  model.exponent);
207  }
208
Definition: Costmap.cpp:392
float minDistance
Definition: types.h:68
armarx::aron::ret
Definition: rw.h:21
Eigen::Isometry3f Pose
Definition: basic_types.h:31
basic_types.h
DateTime timestamp
Definition: Trajectory.h:154
Duration.h
std::vector< LocalTrajectoryPoint > LocalTrajectoryPoints
Definition: Trajectory.h:158
Eigen::MatrixXf Grid
Definition: Costmap.h:48
Eigen::Vector3f fromRos(const Eigen::Vector2d &vec)
Definition: ros_conversions.cpp:30
float exponent
Definition: types.h:70
const SceneBounds & getLocalSceneBounds() const noexcept
Definition: Costmap.cpp:126
Eigen::Isometry2f Pose2D
Definition: basic_types.h:34
Clock.h
Definition: basic_types.h:53
This file is part of ArmarX.
Definition: eigen.cpp:7
float b
Definition: shapes.h:37
bool binaryGrid
Definition: Costmap.h:24
float epsilon
Definition: types.h:69
const Parameters & params() const noexcept
Definition: Costmap.cpp:230
Eigen::Vector2d toRos(const Eigen::Vector3f &vec)
Definition: ros_conversions.cpp:24
armarx::core::time::Duration::SecondsDouble
static Duration SecondsDouble(double seconds)
Constructs a duration in seconds.
Definition: Duration.cpp:90
const Grid & getGrid() const
Definition: Costmap.cpp:236
Eigen::Vector2d toRos2D(const Eigen::Vector2f &vec)
Definition: ros_conversions.cpp:18
std::vector< Eigen::Vector2f > to2D(const std::vector< Eigen::Vector3f > &v)
Definition: eigen.cpp:26
ExpressionException.h
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
LinearVelocity linear
Definition: basic_types.h:55
Eigen::Vector2f min
Definition: types.h:31
float epsilon
Definition: types.h:63
armarx::core::time::Duration
Represents a duration.
Definition: Duration.h:17
float cellSize
How big each cell is in the uniform grid.
Definition: Costmap.h:27
std::vector< Eigen::Vector3f > to3D(const std::vector< Eigen::Vector2f > &v)
Definition: eigen.cpp:11
An axis oriented ellipse with half-axes a and b along the x- and y-axis respectively.
Definition: shapes.h:34
AngularVelocity angular
Definition: basic_types.h:56
armarx::core::time::Clock::Now
static DateTime Now()
Current time on the virtual clock.
Definition: Clock.cpp:97
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
const core::Pose2D & origin() const
Definition: Costmap.h:132
eigen.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
Trajectory.h
const std::vector< GlobalTrajectoryPoint > & points() const
Definition: Trajectory.cpp:732