Skip to content

Commit

Permalink
Reuse nexus_msgs structs, event based publication
Browse files Browse the repository at this point in the history 
Signed-off-by: Luca Della Vedova <[email protected]>
  • Loading branch information
@luca-della-vedova
luca-della-vedova committed Jan 10, 2025
1 parent 64abc78 commit 851082c
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 59 deletions.
121 changes: 76 additions & 45 deletions nexus_visualization/src/workcell_state_visualizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,10 @@ WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& opti
"Setting parameter initial_map_name to %s", _current_level.c_str()
);

auto map_qos = rclcpp::QoS(10).transient_local();
auto transient_qos = rclcpp::QoS(1).transient_local();
_map_sub = this->create_subscription<BuildingMap>(
"/map",
map_qos,
transient_qos,
[=](BuildingMap::ConstSharedPtr msg)
{
RCLCPP_INFO(
Expand All @@ -65,11 +65,12 @@ WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& opti
this->get_logger(),
"Found dispenser [%s]", param.value_string.c_str()
);
WorkcellState workcell_state;
workcell_state.x = vertex.x;
workcell_state.y = vertex.y;
workcell_state.map_name = level.name;
_workcell_states.insert({vertex.name, workcell_state});
WorkcellDescription description;
description.workcell_id = vertex.name;
description.level_name = level.name;
description.location.position.x = vertex.x;
description.location.position.y = vertex.y;
_workcells.insert({vertex.name, Workcell {description, std::nullopt}});
_ids.insert({vertex.name, _next_available_id});
++_next_available_id;
}
Expand All @@ -78,7 +79,6 @@ WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& opti
}
}
initialize_state_subscriptions();
_publish_timer = this->create_wall_timer(1s, std::bind(&WorkcellStateVisualizer::timer_cb, this));
});

_param_sub = this->create_subscription<RvizParam>(
Expand All @@ -90,10 +90,10 @@ WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& opti
return;

_current_level = msg->map_name;
timer_cb();
publish_markers();
});

_marker_pub = this->create_publisher<MarkerArray>("/workcell_markers", 10);
_marker_pub = this->create_publisher<MarkerArray>("/workcell_markers", transient_qos);

RCLCPP_INFO(
this->get_logger(),
Expand All @@ -104,120 +104,151 @@ WorkcellStateVisualizer::WorkcellStateVisualizer(const rclcpp::NodeOptions& opti

void WorkcellStateVisualizer::initialize_state_subscriptions()
{
for (const auto& [name, _] : _workcell_states)
for (const auto& [name, _] : _workcells)
{
_state_subs.emplace_back(nexus::endpoints::WorkcellStateTopic::create_subscription(this, name,
[this](nexus::endpoints::WorkcellStateTopic::MessageType::ConstSharedPtr msg)
{
auto it = _workcell_states.find(msg->workcell_id);
if (it == _workcell_states.end())
auto state_it = _workcells.find(msg->workcell_id);
if (state_it == _workcells.end())
{
RCLCPP_WARN(this->get_logger(), "Received state for workcell [%s] but it was not found in the map", msg->workcell_id.c_str());
return;
}
RCLCPP_INFO(this->get_logger(), "Received state for workcell [%s]", msg->workcell_id.c_str());
auto& workcell_state = it->second;
workcell_state.status = msg->status;
workcell_state.message = msg->message.empty() ? "<None>" : msg->message;
workcell_state.task_id = msg->task_id.empty() ? "<None>" : msg->task_id;
state_it->second.state = *msg;
}));
}
}

void WorkcellStateVisualizer::timer_cb()
void WorkcellStateVisualizer::publish_markers()
{
auto set_body_pose =
[](const WorkcellState& state, Marker& marker)
[](const Workcell& workcell, Marker& marker)
{
marker.pose.position.x = state.x;
marker.pose.position.y = state.y;
marker.pose.position.x = workcell.description.location.position.x;
marker.pose.position.y = workcell.description.location.position.y;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.0;
};

auto set_text_pose =
[](const WorkcellState& state, const double x_offset, Marker& marker)
[](const Workcell& workcell, const double x_offset, Marker& marker)
{
marker.pose.position.x = state.x + x_offset;
marker.pose.position.y = state.y;
marker.pose.position.x = workcell.description.location.position.x + x_offset;
marker.pose.position.y = workcell.description.location.position.y;
marker.pose.position.z = 0.0;
marker.pose.orientation.w = 1.0;
};

auto fill_markers =
auto fill_marker =
[&](
const std::string& name,
const WorkcellState& state,
const Workcell& workcell,
const double radius,
const std::size_t id,
MarkerArray& marker_array)
{
std::size_t id = _ids[name];
Marker body_marker;
body_marker.header.frame_id = "map";
body_marker.header.stamp = this->get_clock()->now();
body_marker.ns = "body";
body_marker.id = id;
body_marker.type = body_marker.CUBE;
body_marker.action = body_marker.MODIFY;
set_body_pose(state, body_marker);
body_marker.action = body_marker.ADD;
set_body_pose(workcell, body_marker);
body_marker.scale.x = radius;
body_marker.scale.y = radius;
body_marker.scale.z = radius;
// TODO(luca) color to match state
Color color;
std::string status;
color.a = 1.0;
if (state.status == 0)
if (!workcell.state.has_value())
{
// Indeterminate, grey
color.r = 0.3;
color.g = 0.3;
color.b = 0.3;
status = "UNKNOWN";
} else if (state.status == WorkcellStateMsg::STATUS_IDLE) {
} else if (workcell.state.value().status == WorkcellState::STATUS_IDLE) {
// Idle, green
color.r = 0.0;
color.g = 0.8;
color.b = 0.0;
status = "IDLE";
} else if (state.status == WorkcellStateMsg::STATUS_BUSY) {
} else if (workcell.state.value().status == WorkcellState::STATUS_BUSY) {
// Busy, yellow
color.r = 0.5;
color.g = 0.5;
color.b = 0.0;
status = "BUSY";
}
body_marker.color = color;
body_marker.lifetime = rclcpp::Duration(std::chrono::seconds(1));

auto text_marker = body_marker;
text_marker.ns = "name";
text_marker.type = text_marker.TEXT_VIEW_FACING;
set_text_pose(state, radius * 2.0, text_marker);
text_marker.text = name + "\n" + "STATUS:" + status + "\n" + "TASK:" + state.task_id + "\n" + "MESSAGE:" + state.message;
set_text_pose(workcell, radius * 2.0, text_marker);
std::string task_id = "<NONE>";
std::string message = "<NONE>";
const auto& state = workcell.state;
if (state.has_value())
{
if (!state.value().message.empty())
{
message = state.value().message;
}
if (!state.value().task_id.empty())
{
task_id = state.value().task_id;
}
}
text_marker.text = name + "\n" + "STATUS:" + status + "\n" + "TASK:" + task_id + "\n" + "MESSAGE:" + message;
text_marker.scale.z = 0.3;

marker_array.markers.push_back(std::move(body_marker));
marker_array.markers.push_back(std::move(text_marker));
};

auto marker_array = std::make_unique<MarkerArray>();
for (const auto& [name, state] : _workcell_states)
auto delete_marker =
[&](
const std::string& name,
MarkerArray& marker_array)
{
if (state.map_name != _current_level)
std::size_t id = _ids[name];
Marker body_marker;
body_marker.header.frame_id = "map";
body_marker.header.stamp = this->get_clock()->now();
body_marker.ns = "body";
body_marker.id = id;
body_marker.type = body_marker.CUBE;
body_marker.action = body_marker.DELETE;
auto text_marker = body_marker;
text_marker.ns = "name";
text_marker.type = text_marker.TEXT_VIEW_FACING;
marker_array.markers.push_back(std::move(body_marker));
marker_array.markers.push_back(std::move(text_marker));
};

MarkerArray marker_array;
for (const auto& [name, workcell] : _workcells)
{
if (workcell.description.level_name != _current_level)
{
delete_marker(name, marker_array);
}
continue;

double radius = 1.0;
fill_markers(
fill_marker(
name,
state,
workcell,
radius,
_ids[name],
*marker_array
marker_array
);
}
if (!marker_array->markers.empty())
_marker_pub->publish(std::move(marker_array));
if (!marker_array.markers.empty())
_marker_pub->publish(marker_array);
}
}

Expand Down
21 changes: 7 additions & 14 deletions nexus_visualization/src/workcell_state_visualizer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@ class WorkcellStateVisualizer : public rclcpp::Node
public:
using BuildingMap = rmf_building_map_msgs::msg::BuildingMap;
using RvizParam = rmf_visualization_msgs::msg::RvizParam;
using WorkcellStateMsg = nexus_orchestrator_msgs::msg::WorkcellState;
using WorkcellDescription = nexus_orchestrator_msgs::msg::WorkcellDescription;
using WorkcellState = nexus_orchestrator_msgs::msg::WorkcellState;
using Marker = visualization_msgs::msg::Marker;
using MarkerArray = visualization_msgs::msg::MarkerArray;
using Color = std_msgs::msg::ColorRGBA;
Expand All @@ -50,25 +51,17 @@ class WorkcellStateVisualizer : public rclcpp::Node
const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

private:
struct WorkcellState {
double x;
double y;
std::string map_name;

// Follows enum in WorkcellState.msg
// 0 indefinite, 1 idle, 2 busy
uint8_t status = 0;
std::string task_id = "<None>";
std::string message = "<None>";
struct Workcell {
WorkcellDescription description;
std::optional<WorkcellState> state;
};

void initialize_state_subscriptions();

void timer_cb();
void publish_markers();

std::string _current_level;

rclcpp::TimerBase::SharedPtr _publish_timer;
rclcpp::Subscription<BuildingMap>::SharedPtr _map_sub;
rclcpp::Subscription<RvizParam>::SharedPtr _param_sub;
std::vector<rclcpp::Subscription<nexus::endpoints::WorkcellStateTopic::MessageType>::SharedPtr> _state_subs;
Expand All @@ -78,7 +71,7 @@ class WorkcellStateVisualizer : public rclcpp::Node
std::unordered_map<std::string, std::size_t> _ids;
std::size_t _next_available_id;

std::unordered_map<std::string, WorkcellState> _workcell_states;
std::unordered_map<std::string, Workcell> _workcells;
};

}
Expand Down

0 comments on commit 851082c

Please sign in to comment.