Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
Classes | Typedefs | Functions
carma_wm::collision_detection Namespace Reference

Classes

struct  MovingObject
 

Typedefs

typedef boost::geometry::model::point< double, 2, boost::geometry::cs::cartesian > point_t
 
typedef boost::geometry::model::polygon< point_t > polygon_t
 

Functions

std::vector< carma_perception_msgs::msg::RoadwayObstacle > WorldCollisionDetection (const carma_perception_msgs::msg::RoadwayObstacleList &rwol, const carma_planning_msgs::msg::TrajectoryPlan &tp, const geometry_msgs::msg::Vector3 &size, const geometry_msgs::msg::Twist &velocity)
 Main collision detection function to be called when needed to check for collision detection of the vehicle with the current trajectory plan and the current world objects. More...
 
collision_detection::MovingObject ConvertRoadwayObstacleToMovingObject (const carma_perception_msgs::msg::RoadwayObstacle &rwo)
 Convert RodwayObstable object to the collision_detection::MovingObject. More...
 
collision_detection::MovingObject ConvertVehicleToMovingObject (const carma_planning_msgs::msg::TrajectoryPlan &tp, const geometry_msgs::msg::Vector3 &size, const geometry_msgs::msg::Twist &veloctiy)
 Creates collision_detection::MovingObject for the host vehicle using the veloctiy, size, TrajectoryPlan. More...
 
collision_detection::MovingObject PredictObjectPosition (collision_detection::MovingObject const &op, __uint64_t target_time)
 .function is to create a monving object with a polygon that represents area that object is going to allocated until a given time by creating a convex hull around the future polygons More...
 
bool CheckPolygonIntersection (collision_detection::MovingObject const &ob_1, collision_detection::MovingObject const &ob_2)
 .check Intersection between polygons More...
 
bool DetectCollision (collision_detection::MovingObject const &ob_1, collision_detection::MovingObject const &ob_2, __uint64_t target_time)
 .function is used in WorldCollisionDetection to detection collision between two Moving Object More...
 
template<class P >
P ObjectToBoostPolygon (const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size)
 function to create a polygon representing and object More...
 

Typedef Documentation

◆ point_t

typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> carma_wm::collision_detection::point_t

Definition at line 42 of file collision_detection.hpp.

◆ polygon_t

typedef boost::geometry::model::polygon<point_t> carma_wm::collision_detection::polygon_t

Definition at line 43 of file collision_detection.hpp.

Function Documentation

◆ CheckPolygonIntersection()

bool carma_wm::collision_detection::CheckPolygonIntersection ( collision_detection::MovingObject const &  ob_1,
collision_detection::MovingObject const &  ob_2 
)

.check Intersection between polygons

Definition at line 176 of file collision_detection.cpp.

176 {
177
178 std::deque<polygon_t> output;
179
180 boost::geometry::intersection(ob_1.object_polygon, ob_2.object_polygon, output);
181 // std::cout << boost::geometry::wkt(output) << std::endl;
182
183 if(output.size() > 0){
184 return true;
185 }
186
187 return false;
188 }

References carma_wm::collision_detection::MovingObject::object_polygon.

Referenced by DetectCollision().

Here is the caller graph for this function:

◆ ConvertRoadwayObstacleToMovingObject()

collision_detection::MovingObject carma_wm::collision_detection::ConvertRoadwayObstacleToMovingObject ( const carma_perception_msgs::msg::RoadwayObstacle &  rwo)

Convert RodwayObstable object to the collision_detection::MovingObject.

Parameters
rwoA RoadwayObstacle

Definition at line 92 of file collision_detection.cpp.

92 {
93
94 collision_detection::MovingObject mo;
95
96 mo.object_polygon = ObjectToBoostPolygon<polygon_t>(rwo.object.pose.pose, rwo.object.size);
97
98 std::tuple <__uint64_t,polygon_t> current_pose(0 , mo.object_polygon);
99 mo.fp.push_back(current_pose);
100
101 // Add future polygons for roadway obstacle
102 for (auto i : rwo.object.predictions){
103 std::tuple <__uint64_t,polygon_t> future_object((i.header.stamp.sec*1e9 + i.header.stamp.nanosec) / 1000000, ObjectToBoostPolygon<polygon_t>(i.predicted_position, rwo.object.size));
104
105 mo.fp.push_back(future_object);
106 }
107
108 mo.linear_velocity = rwo.object.velocity.twist.linear;
109
110 return mo;
111 }
process_bag.i
int i
Definition: process_bag.py:59
carma_wm::collision_detection::MovingObject
Definition: collision_detection.hpp:46
carma_wm::collision_detection::MovingObject::fp
std::vector< std::tuple< __uint64_t, polygon_t > > fp
Definition: collision_detection.hpp:49
carma_wm::collision_detection::MovingObject::object_polygon
polygon_t object_polygon
Definition: collision_detection.hpp:47
carma_wm::collision_detection::MovingObject::linear_velocity
geometry_msgs::msg::Vector3 linear_velocity
Definition: collision_detection.hpp:48

References carma_wm::collision_detection::MovingObject::fp, process_bag::i, carma_wm::collision_detection::MovingObject::linear_velocity, and carma_wm::collision_detection::MovingObject::object_polygon.

◆ ConvertVehicleToMovingObject()

collision_detection::MovingObject carma_wm::collision_detection::ConvertVehicleToMovingObject ( const carma_planning_msgs::msg::TrajectoryPlan &  tp,
const geometry_msgs::msg::Vector3 &  size,
const geometry_msgs::msg::Twist &  veloctiy 
)

Creates collision_detection::MovingObject for the host vehicle using the veloctiy, size, TrajectoryPlan.

Parameters
tpThe TrajectoryPlan of the host vehicle
sizeThe size of the host vehicle defined in meters
veloctiyof the host vehicle

Definition at line 113 of file collision_detection.cpp.

113 {
114
115 collision_detection::MovingObject v;
116
117 geometry_msgs::msg::Pose pose;
118 pose.position.x = tp.trajectory_points[0].x;
119 pose.position.y = tp.trajectory_points[0].y;
120
121 Eigen::Vector2d vehicle_vector = {tp.trajectory_points[1].x - tp.trajectory_points[0].x , tp.trajectory_points[1].y - tp.trajectory_points[0].y};
122
123 Eigen::Vector2d x_axis = {1, 0};
124
125 double yaw = std::acos(vehicle_vector.dot(x_axis)/(vehicle_vector.norm() * x_axis.norm()));
126
127 tf2::Quaternion vehicle_orientation;
128 vehicle_orientation.setRPY(0, 0, yaw);
129
130 pose.orientation.x = vehicle_orientation.getX();
131 pose.orientation.y = vehicle_orientation.getY();
132 pose.orientation.z = vehicle_orientation.getZ();
133 pose.orientation.w = vehicle_orientation.getW();
134
135 v.object_polygon = ObjectToBoostPolygon<polygon_t>(pose, size);
136 v.linear_velocity = velocity.linear;
137
138 for(size_t i=1; i < tp.trajectory_points.size() - 1; i++){
139
140 vehicle_vector = {tp.trajectory_points[i + 1].x - tp.trajectory_points[i].x , tp.trajectory_points[i+1].y - tp.trajectory_points[i].y};
141 yaw = std::acos(vehicle_vector.dot(x_axis)/(vehicle_vector.norm() * x_axis.norm()));
142
143 tf2::Quaternion orientation;
144 orientation.setRPY(0, 0, yaw);
145
146 geometry_msgs::msg::Pose trajectory_pose;
147 trajectory_pose.position.x = tp.trajectory_points[i].x;
148 trajectory_pose.position.y = tp.trajectory_points[i].y;
149
150 trajectory_pose.orientation.x = orientation.getX();
151 trajectory_pose.orientation.y = orientation.getY();
152 trajectory_pose.orientation.z = orientation.getZ();
153 trajectory_pose.orientation.w = orientation.getW();
154
155 std::tuple <__uint64_t,polygon_t> future_object((tp.trajectory_points[i].target_time.sec*1e9 + tp.trajectory_points[i].target_time.nanosec),ObjectToBoostPolygon<polygon_t>(trajectory_pose, size));
156
157 v.fp.push_back(future_object);
158 }
159
160 return v;
161 }

References carma_wm::collision_detection::MovingObject::fp, process_bag::i, carma_wm::collision_detection::MovingObject::linear_velocity, and carma_wm::collision_detection::MovingObject::object_polygon.

◆ DetectCollision()

bool carma_wm::collision_detection::DetectCollision ( collision_detection::MovingObject const &  ob_1,
collision_detection::MovingObject const &  ob_2,
__uint64_t  target_time 
)

.function is used in WorldCollisionDetection to detection collision between two Moving Object

Definition at line 163 of file collision_detection.cpp.

163 {
164
165 collision_detection::MovingObject ob_1_after = PredictObjectPosition(ob_1,target_time);
166
167 collision_detection::MovingObject ob_2_after = PredictObjectPosition(ob_2,target_time);
168
169 if(CheckPolygonIntersection(ob_1_after, ob_2_after)){
170 return true;
171 }
172
173 return false;
174 }
carma_wm::collision_detection::CheckPolygonIntersection
bool CheckPolygonIntersection(collision_detection::MovingObject const &ob_1, collision_detection::MovingObject const &ob_2)
.check Intersection between polygons
Definition: collision_detection.cpp:176
carma_wm::collision_detection::PredictObjectPosition
collision_detection::MovingObject PredictObjectPosition(collision_detection::MovingObject const &op, __uint64_t target_time)
.function is to create a monving object with a polygon that represents area that object is going to a...
Definition: collision_detection.cpp:190

References CheckPolygonIntersection(), and PredictObjectPosition().

Here is the call graph for this function:

◆ ObjectToBoostPolygon()

template<class P >
P carma_wm::collision_detection::ObjectToBoostPolygon ( const geometry_msgs::msg::Pose &  pose,
const geometry_msgs::msg::Vector3 &  size 
)

function to create a polygon representing and object

Definition at line 221 of file collision_detection.cpp.

221 {
222
223 tf2::Transform object_tf;
224 tf2::fromMsg(pose, object_tf);
225
226 double half_x_bound = size.x / 2;
227 double half_y_bound = size.y / 2;
228
229 // 4 corners of the object starting with upper left and moving in clockwise direction in pose frame
230 tf2::Vector3 obj_p1(half_x_bound, half_y_bound, 0);
231 tf2::Vector3 obj_p2(half_x_bound, -half_y_bound, 0);
232 tf2::Vector3 obj_p3(-half_x_bound, -half_y_bound, 0);
233 tf2::Vector3 obj_p4(-half_x_bound, half_y_bound, 0);
234
235 tf2::Vector3 obj_p1_map = object_tf * obj_p1;
236 tf2::Vector3 obj_p2_map = object_tf * obj_p2;
237 tf2::Vector3 obj_p3_map = object_tf * obj_p3;
238 tf2::Vector3 obj_p4_map = object_tf * obj_p4;
239
240 point_t p1(obj_p1_map.getX(), obj_p1_map.getY());
241 point_t p2(obj_p2_map.getX(), obj_p2_map.getY());
242 point_t p3(obj_p3_map.getX(), obj_p3_map.getY());
243 point_t p4(obj_p4_map.getX(), obj_p4_map.getY());
244
245 P p;
246 p.outer().push_back(p1);
247 p.outer().push_back(p2);
248 p.outer().push_back(p3);
249 p.outer().push_back(p4);
250
251 return p;
252 }
carma_wm::collision_detection::point_t
boost::geometry::model::point< double, 2, boost::geometry::cs::cartesian > point_t
Definition: collision_detection.hpp:42

◆ PredictObjectPosition()

collision_detection::MovingObject carma_wm::collision_detection::PredictObjectPosition ( collision_detection::MovingObject const &  op,
__uint64_t  target_time 
)

.function is to create a monving object with a polygon that represents area that object is going to allocated until a given time by creating a convex hull around the future polygons

Parameters
opa MovingObject TODO this function can be optimize due the fact that in large target_time returning the entire volum for path will result in false positive.

Definition at line 190 of file collision_detection.cpp.

190 {
191
192 int union_polygon_size = 0;
193 for (auto i : op.fp){
194 if( std::get<0>(i) <= target_time) {
195 union_polygon_size = union_polygon_size + std::get<1>(i).outer().size();
196 }
197 }
198
199 std::vector<point_t> union_future_polygon_points;
200 union_future_polygon_points.reserve(union_polygon_size);
201
202 for (auto i : op.fp){
203 if( std::get<0>(i) <= target_time) {
204 union_future_polygon_points.insert( union_future_polygon_points.end(), std::get<1>(i).outer().begin(), std::get<1>(i).outer().end());
205 }
206 }
207
208 polygon_t union_polygon;
209 boost::geometry::assign_points(union_polygon, union_future_polygon_points);
210
211 polygon_t hull_polygon;
212 boost::geometry::convex_hull(union_polygon, hull_polygon);
213
214 std::vector<std::tuple <__uint64_t, collision_detection::polygon_t>> no_future;
215 collision_detection::MovingObject output_object = {hull_polygon, op.linear_velocity, no_future};
216
217 return output_object;
218 }
carma_wm::collision_detection::polygon_t
boost::geometry::model::polygon< point_t > polygon_t
Definition: collision_detection.hpp:43

References carma_wm::collision_detection::MovingObject::fp, process_bag::i, and carma_wm::collision_detection::MovingObject::linear_velocity.

Referenced by DetectCollision().

Here is the caller graph for this function:

◆ WorldCollisionDetection()

std::vector< carma_perception_msgs::msg::RoadwayObstacle > carma_wm::collision_detection::WorldCollisionDetection ( const carma_perception_msgs::msg::RoadwayObstacleList &  rwol,
const carma_planning_msgs::msg::TrajectoryPlan &  tp,
const geometry_msgs::msg::Vector3 &  size,
const geometry_msgs::msg::Twist &  velocity 
)

Main collision detection function to be called when needed to check for collision detection of the vehicle with the current trajectory plan and the current world objects.

Main Function for the CollisionChecking interfacing.

Parameters
sizeThe size of the host vehicle defined in meters
rwolThe list of Roadway Obstacle
tpThe TrajectoryPlan of the host vehicle
sizeThe size of the host vehicle defined in meters
velocityof the host vehicle m/s
Returns
A list of obstacles the provided trajectory plan collides with

Definition at line 26 of file collision_detection.cpp.

27 {
28
29
30 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "WorldCollisionDetection");
31
32 std::vector<carma_perception_msgs::msg::RoadwayObstacle> rwo_collison;
33
34
35 for (auto i : rwol.roadway_obstacles) {
36
37 for (auto j : i.object.predictions) {
38
39 for(size_t k=0; k < tp.trajectory_points.size(); k++) {
40
41 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "in for loop");
42
43 double distancex = (tp.trajectory_points[k].x - j.predicted_position.position.x)*(tp.trajectory_points[k].x - j.predicted_position.position.x);
44 double distancey = (tp.trajectory_points[k].y - j.predicted_position.position.y)*(tp.trajectory_points[k].y - j.predicted_position.position.y);
45
46 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "tp.trajectory_points[k].x");
47 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), tp.trajectory_points[k].x);
48
49 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "j.predicted_position.position.x");
50 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), j.predicted_position.position.x);
51
52 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "tp.trajectory_points[k].y");
53 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), tp.trajectory_points[k].y);
54
55 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), "j.predicted_position.position.y");
56 RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::collision_detection"), j.predicted_position.position.y);
57
58
59 double calcdistance = sqrt(abs(distancex + distancey));
60
61 rclcpp::Duration diff= rclcpp::Time(j.header.stamp) - rclcpp::Time(tp.trajectory_points[k].target_time);
62
63 double timediff = diff.seconds();
64
65 double x = (i.object.size.x - size.x)*(i.object.size.x - size.x);
66 double y = (i.object.size.y - size.y)*(i.object.size.y - size.y);
67
68 double car_t_x = tp.trajectory_points[k].x - tp.trajectory_points[0].x / velocity.linear.x;
69 // double car_t_y = tp.trajectory_points[k].y - tp.trajectory_points[0].y/velocity.linear.y;
70
71 double object_t_x = j.predicted_position.position.x - i.object.predictions[0].predicted_position.position.x / i.object.velocity.twist.linear.x;
72 // double object_t_y = j.predicted_position.position.y - i.object.predictions[0].predicted_position.position.y / j.predicted_velocity.linear.y;
73
74
75 if(timediff <= 5) {
76 if(calcdistance <= sqrt(x - y) ) {
77 rwo_collison.push_back(i);
78 break;
79 }
80 }
81 }
82 }
83
84
85 }
86
87
88
89 return rwo_collison;
90 }
process_traj_logs.y
y
Definition: process_traj_logs.py:129
process_traj_logs.x
x
Definition: process_traj_logs.py:128

References process_bag::i, process_traj_logs::x, and process_traj_logs::y.