Skip to content

Commit

Permalink
feat(parameters_visualization): rename topic name, cleaning up, refac…
Browse files Browse the repository at this point in the history 
…toring, and some fixes
  • Loading branch information
@ahmeddesokyebrahim
ahmeddesokyebrahim committed Sep 2, 2024
1 parent d804714 commit 648ea87
Show file tree
Hide file tree
Showing 5 changed files with 15 additions and 131 deletions.
2 changes: 1 addition & 1 deletion ...io/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode,
bool waiting_for_engagement_to_be_completed = false; // NOTE: DIRTY HACK!!!
std_msgs::msg::String params_str = std_msgs::msg::String();
std_msgs::msg::String test_iteration_params_str = std_msgs::msg::String();
public:
OPENSCENARIO_INTERPRETER_PUBLIC
Expand Down
20 changes: 9 additions & 11 deletions openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ namespace openscenario_interpreter
Interpreter::Interpreter(const rclcpp::NodeOptions & options)
: rclcpp_lifecycle::LifecycleNode("openscenario_interpreter", options),
publisher_of_context(create_publisher<Context>("context", rclcpp::QoS(1).transient_local())),
publisher_of_params(create_publisher<std_msgs::msg::String>("/iteration_paramters", rclcpp::QoS(1).transient_local())),
publisher_of_params(create_publisher<std_msgs::msg::String>("test_iteration_parameters", rclcpp::QoS(1).transient_local())),
local_frame_rate(30),
local_real_time_factor(1.0),
osc_path(""),
Expand Down Expand Up @@ -123,16 +123,14 @@ auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result

if (script->category.is<ScenarioDefinition>()) {
scenarios = {std::dynamic_pointer_cast<ScenarioDefinition>(script->category)};
params_str.data.clear();
params_str.data = "Parameter declarations for this scenario iteration:\n";
std::cout << "Parameter declarations for this scenario iteration:" << std::endl;
// get and log parameters used in this test iteration
test_iteration_params_str.data.clear();
test_iteration_params_str.data = "Parameter declarations for this scenario iteration:\n";
for (const auto & pd : currentScenarioDefinition()->parameter_declarations) {
params_str.data.append(pd.name);
params_str.data.append(" = ");
params_str.data.append(pd.value);
params_str.data.append("\n");
std::cout << pd.name << " = " << pd.value << std::endl;
test_iteration_params_str.data.append(pd.name + " = " + pd.value + "\n");
}
RCLCPP_INFO_STREAM(get_logger(), test_iteration_params_str.data);

} else if (script->category.is<ParameterValueDistribution>()) {
throw Error(
"ParameterValueDistribution cannot be processed by openscenario_interpreter alone. "
Expand Down Expand Up @@ -191,7 +189,7 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
withExceptionHandler(
[this](auto &&...) {
publishCurrentContext();
publisher_of_params->publish(params_str);
publisher_of_params->publish(test_iteration_params_str);
deactivate();
},
[this]() {
Expand All @@ -213,7 +211,7 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
SimulatorCore::update();

publishCurrentContext();
publisher_of_params->publish(params_str);
publisher_of_params->publish(test_iteration_params_str);
});
});
};
Expand Down
28 changes: 1 addition & 27 deletions ...io_visualization_condition_groups_plugin/openscenario_visualization_parameters_plugin.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,27 +46,6 @@

namespace openscenario_visualization
{
// struct Condition
// {
// std::string current_evaluation;
// std::string current_value;
// std::string type;
// };

// struct ConditionGroup
// {
// std::vector<Condition> conditions;
// };

// struct ConditionGroups
// {
// std::string groups_name;
// std::vector<ConditionGroup> condition_groups;
// };

// using nlohmann::json;
// using openscenario_interpreter_msgs::msg::Context;
// using ConditionGroupsCollection = std::vector<ConditionGroups>;

class VisualizationParametersDisplay : public rviz_common::Display
{
Expand Down Expand Up @@ -98,13 +77,8 @@ private Q_SLOTS:
rviz_common::properties::FloatProperty * property_value_scale_;

private:
//void loadConditionGroups(const std_msgs::msg::String::ConstSharedPtr msg_ptr);
//void processStory(const YAML::Node & story);
//void processManeuver(const YAML::Node & maneuver);
//void processEvent(const YAML::Node & event);
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr simulation_context_sub_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr test_iteration_params_sub_;
std_msgs::msg::String::ConstSharedPtr last_msg_ptr_;
//std::shared_ptr<ConditionGroupsCollection> condition_groups_collection_ptr_;
};

} // namespace openscenario_visualization
Expand Down
94 changes: 3 additions & 91 deletions ...io_visualization_condition_groups_plugin/openscenario_visualization_parameters_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ VisualizationParametersDisplay::VisualizationParametersDisplay()
const int width = static_cast<int>(std::round(2000 * scale));

property_topic_name_ = new rviz_common::properties::StringProperty(
"Topic", "/iteration_paramters", "The topic on which to publish simulation context.", this,
"Topic", "/simulation/test_iteration_parameters", "The topic on which to test iteration parameters are published.", this,
SLOT(updateTopic()), this);
property_text_color_ = new rviz_common::properties::ColorProperty(
"Text Color", QColor(255, 255, 255), "text color", this, SLOT(updateVisualization()), this);
Expand Down Expand Up @@ -148,13 +148,13 @@ void VisualizationParametersDisplay::subscribe()
std::string topic_name = property_topic_name_->getStdString();
if (topic_name.length() > 0 && topic_name != "/") {
rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node();
simulation_context_sub_ = raw_node->create_subscription<std_msgs::msg::String>(
test_iteration_params_sub_ = raw_node->create_subscription<std_msgs::msg::String>(
topic_name, rclcpp::QoS{1}.transient_local(),
std::bind(&VisualizationParametersDisplay::processMessage, this, std::placeholders::_1));
}
}

void VisualizationParametersDisplay::unsubscribe() { simulation_context_sub_.reset(); }
void VisualizationParametersDisplay::unsubscribe() { test_iteration_params_sub_.reset(); }

void VisualizationParametersDisplay::processMessage(const std_msgs::msg::String::ConstSharedPtr msg_ptr)
{
Expand All @@ -178,22 +178,6 @@ void VisualizationParametersDisplay::processMessage(const std_msgs::msg::String:
font.setBold(true);
painter.setFont(font);

// loadConditionGroups(msg_ptr);

// std::ostringstream context_ss;
// for (const auto & condition_groups : *condition_groups_collection_ptr_) {
// context_ss << std::fixed << std::setprecision(0)
// << "Condition Groups Name: " << condition_groups.groups_name << std::endl;
// for (const auto & condition_group : condition_groups.condition_groups) {
// for (const auto & condition : condition_group.conditions) {
// context_ss << " Current Evaluation: " << condition.current_evaluation << std::endl
// << " Current Value: " << condition.current_value << std::endl
// << " Type: " << condition.type << std::endl;
// }
// }
// context_ss << std::endl;
// }

painter.drawText(
property_left_->getInt(), property_top_->getInt(), overlay_->getTextureWidth(),
overlay_->getTextureHeight(), Qt::AlignLeft | Qt::AlignTop, msg_ptr->data.c_str());
Expand All @@ -216,78 +200,6 @@ void VisualizationParametersDisplay::updateVisualization()
}
}

// void VisualizationParametersDisplay::loadConditionGroups(const std_msgs::msg::String::ConstSharedPtr msg_ptr)
// {
// if (!msg_ptr) return;

// YAML::Node data;
// try {
// data = YAML::Load(msg_ptr->data);
// } catch (const std::exception & e) {
// throw std::runtime_error(std::string("Failed to load YAML: ") + e.what());
// }

// condition_groups_collection_ptr_->clear();

// auto stories = data["OpenSCENARIO"]["Storyboard"]["Story"];
// for (const auto & story : stories) {
// processStory(story);
// }
// }

// void VisualizationParametersDisplay::processStory(const YAML::Node & story_node)
// {
// for (const auto & act : story_node["Act"]) {
// for (const auto & maneuver_group : act["ManeuverGroup"]) {
// for (const auto & maneuver : maneuver_group["Maneuver"]) {
// processManeuver(maneuver);
// }
// }
// }
// }

// void VisualizationParametersDisplay::processManeuver(const YAML::Node & maneuver_node)
// {
// for (const auto & event : maneuver_node["Event"]) {
// if (event["StartTrigger"] && event["StartTrigger"]["ConditionGroup"]) {
// processEvent(event);
// }
// }
// }

// void VisualizationParametersDisplay::processEvent(const YAML::Node & event_node)
// {
// std::string event_name;
// try {
// event_name = event_node["name"].as<std::string>();
// } catch (const YAML::BadConversion & e) {
// event_name = "";
// }
// if (event_name.empty()) {
// event_name = "name is not defined";
// }

// ConditionGroups condition_groups;
// condition_groups.groups_name = event_name;

// for (const auto & condition_group_node : event_node["StartTrigger"]["ConditionGroup"]) {
// ConditionGroup condition_group;

// for (const auto & condition_node : condition_group_node["Condition"]) {
// Condition condition_msg;
// condition_msg.current_evaluation = condition_node["currentEvaluation"].as<std::string>();
// condition_msg.current_value = condition_node["currentValue"].as<std::string>();
// condition_msg.type = condition_node["type"].as<std::string>();

// condition_group.conditions.push_back(condition_msg);
// }

// condition_groups.condition_groups.push_back(condition_group);
// }

// condition_groups_collection_ptr_->push_back(condition_groups);
// }

} // namespace openscenario_visualization

#include <pluginlib/class_list_macros.hpp>
Expand Down
2 changes: 1 addition & 1 deletion simulation/traffic_simulator/config/scenario_simulator_v2.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2871,7 +2871,7 @@ Visualization Manager:
Name: Test Iteration Parameters
Text Color: 255; 255; 255
Top: 0
Topic: /iteration_paramters
Topic: /simulation/test_iteration_parameters
Value: true
Value Scale: 11
Width: 1000
Expand Down

0 comments on commit 648ea87

Please sign in to comment.