Skip to content

Commit 2a734d9

Browse files
committed
draft
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 942ce25 commit 2a734d9

File tree

4 files changed

+90
-172
lines changed

4 files changed

+90
-172
lines changed

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

+34-72
Original file line numberDiff line numberDiff line change
@@ -171,48 +171,52 @@ struct PlanningControlModeParams
171171
template <typename MessageType>
172172
struct PublisherVariables
173173
{
174-
double period_ns;
174+
double period_ns{0.0};
175175
typename MessageType::SharedPtr empty_area_message;
176176
typename MessageType::SharedPtr object_spawned_message;
177177
typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
178178
rclcpp::TimerBase::SharedPtr timer;
179179
};
180180

181-
template <typename T>
182-
struct PublisherVariablesMessageTypeExtractor;
183-
184-
template <typename MessageType>
185-
struct PublisherVariablesMessageTypeExtractor<PublisherVariables<MessageType>>
186-
{
187-
using Type = MessageType;
188-
};
189-
190-
template <typename T, typename = std::void_t<>>
191-
struct has_header : std::false_type
181+
struct PublisherVarAccessor
192182
{
193-
};
183+
template <typename T, typename = std::void_t<>>
184+
struct has_header : std::false_type
185+
{
186+
};
194187

195-
template <typename T>
196-
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
197-
{
198-
};
188+
template <typename T>
189+
struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
190+
{
191+
};
192+
template <typename T, typename = std::void_t<>>
193+
struct has_stamp : std::false_type
194+
{
195+
};
199196

200-
struct PublisherVarAccessor
201-
{
202-
// Method to set header time if available
203197
template <typename T>
204-
void setHeaderTimeIfAvailable(T & publisherVar, const rclcpp::Time & time)
198+
struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
205199
{
206-
if constexpr (has_header<typename T::MessageType>::value) {
207-
if (publisherVar.empty_area_message) {
208-
publisherVar.empty_area_message->header.stamp = time;
209-
}
210-
if (publisherVar.object_spawned_message) {
211-
publisherVar.object_spawned_message->header.stamp = time;
212-
}
200+
};
201+
template <typename MessageType>
202+
void publishWithCurrentTime(
203+
const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
204+
const bool is_object_spawned) const
205+
{
206+
std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
207+
208+
if (is_object_spawned) {
209+
*msg_to_be_published = *publisherVar.object_spawned_message;
210+
} else {
211+
*msg_to_be_published = *publisherVar.empty_area_message;
212+
}
213+
if constexpr (has_header<MessageType>::value) {
214+
msg_to_be_published->header.stamp = current_time;
215+
} else if constexpr (has_stamp<MessageType>::value) {
216+
msg_to_be_published->stamp = current_time;
213217
}
218+
publisherVar.publisher->publish(std::move(msg_to_be_published));
214219
}
215-
216220
// Set Period
217221
template <typename T>
218222
void setPeriod(T & publisherVar, double newPeriod)
@@ -227,62 +231,19 @@ struct PublisherVarAccessor
227231
return publisherVar.period_ns;
228232
}
229233

230-
// Set Empty Area Message
231-
template <typename T, typename Message>
232-
void setEmptyAreaMessage(T & publisherVar, typename Message::SharedPtr message)
233-
{
234-
publisherVar.empty_area_message = message;
235-
}
236-
237234
// Get Empty Area Message
238235
template <typename T>
239236
std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
240237
{
241238
return std::static_pointer_cast<void>(publisherVar.empty_area_message);
242239
}
243240

244-
// Set Object Spawned Message
245-
template <typename T, typename Message>
246-
void setObjectSpawnedMessage(T & publisherVar, typename Message::SharedPtr message)
247-
{
248-
publisherVar.object_spawned_message = message;
249-
}
250-
251241
// Get Object Spawned Message
252242
template <typename T>
253243
std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
254244
{
255245
return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
256246
}
257-
258-
// Set Publisher
259-
template <typename T, typename PublisherType>
260-
void setPublisher(
261-
T & publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher)
262-
{
263-
publisherVar.publisher = publisher;
264-
}
265-
266-
// Get Publisher
267-
template <typename T>
268-
std::shared_ptr<void> getPublisher(const T & publisherVar) const
269-
{
270-
return std::static_pointer_cast<void>(publisherVar.publisher);
271-
}
272-
273-
// Set Timer
274-
template <typename T>
275-
void setTimer(T & publisherVar, rclcpp::TimerBase::SharedPtr newTimer)
276-
{
277-
publisherVar.timer = newTimer;
278-
}
279-
280-
// Get Timer
281-
template <typename T>
282-
std::shared_ptr<void> getTimer(const T & publisherVar) const
283-
{
284-
return std::static_pointer_cast<void>(publisherVar.timer);
285-
}
286247
};
287248

288249
using PublisherVariablesVariant = std::variant<
@@ -410,6 +371,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
410371
std::unordered_map<std::string, BufferVariant> message_buffers_;
411372
std::optional<rclcpp::Time> last_test_environment_init_time_;
412373
std::optional<rclcpp::Time> spawn_cmd_time_;
374+
std::atomic<bool> is_object_spawned_{false};
413375
bool is_test_environment_created_{false};
414376
bool is_ego_initialized_{false};
415377
bool is_route_set_{false};

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

+17-17
Original file line numberDiff line numberDiff line change
@@ -7,23 +7,23 @@
77
<arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
88
<arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
99

10-
<group if="$(eval &quot;'$(var running_mode)'=='perception_planning'&quot;)">
11-
<include file="$(find-pkg-share autoware_launch)/launch/e2e_simulator.launch.xml">
12-
<arg name="map_path" value="$(var map_path)"/>
13-
<arg name="vehicle_model" value="$(var vehicle_model)"/>
14-
<arg name="sensor_model" value="$(var sensor_model)"/>
15-
<arg name="use_sim_time" value="false"/>
16-
<arg name="enable_image_decompressor" value="false"/>
17-
</include>
18-
</group>
19-
<group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">
20-
<include file="$(find-pkg-share autoware_launch)/launch/planning_simulator.launch.xml">
21-
<arg name="map_path" value="$(var map_path)"/>
22-
<arg name="vehicle_model" value="$(var vehicle_model)"/>
23-
<arg name="sensor_model" value="$(var sensor_model)"/>
24-
<arg name="use_sim_time" value="false"/>
25-
</include>
26-
</group>
10+
<!-- <group if="$(eval &quot;'$(var running_mode)'=='perception_planning'&quot;)">-->
11+
<!-- <include file="$(find-pkg-share autoware_launch)/launch/e2e_simulator.launch.xml">-->
12+
<!-- <arg name="map_path" value="$(var map_path)"/>-->
13+
<!-- <arg name="vehicle_model" value="$(var vehicle_model)"/>-->
14+
<!-- <arg name="sensor_model" value="$(var sensor_model)"/>-->
15+
<!-- <arg name="use_sim_time" value="false"/>-->
16+
<!-- <arg name="enable_image_decompressor" value="false"/>-->
17+
<!-- </include>-->
18+
<!-- </group>-->
19+
<!-- <group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">-->
20+
<!-- <include file="$(find-pkg-share autoware_launch)/launch/planning_simulator.launch.xml">-->
21+
<!-- <arg name="map_path" value="$(var map_path)"/>-->
22+
<!-- <arg name="vehicle_model" value="$(var vehicle_model)"/>-->
23+
<!-- <arg name="sensor_model" value="$(var sensor_model)"/>-->
24+
<!-- <arg name="use_sim_time" value="false"/>-->
25+
<!-- </include>-->
26+
<!-- </group>-->
2727

2828
<node_container pkg="rclcpp_components" exec="component_container_mt" name="reaction_analyzer_container" namespace="" args="" output="screen">
2929
<composable_node pkg="reaction_analyzer" plugin="reaction_analyzer::ReactionAnalyzerNode" name="reaction_analyzer" namespace="">

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@
4040
perception_planning:
4141
path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
4242
path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
43-
publish_pointcloud_synchronically: false
43+
publish_pointcloud_synchronically: true
4444
synchronic_pointcloud_publishing_period: 0.1 # s only used if publish_pointcloud_synchronically is true
4545
spawn_time_after_init: 10.0 # s
4646
planning_control:

0 commit comments

Comments
 (0)