diff --git a/CMakeLists.txt b/CMakeLists.txt index b15c0bb..584f42a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,7 +195,7 @@ set_target_properties( install(TARGETS pyhpp_plot_graph_viewer DESTINATION "${PYTHON_SITELIB}/pyhpp_plot") -file(WRITE "${CMAKE_BINARY_DIR}/pyhpp_plot/__init__.py" - "from .graph_viewer import show_graph\n") -install(FILES "${CMAKE_BINARY_DIR}/pyhpp_plot/__init__.py" +# Install Python modules +install(FILES "${CMAKE_SOURCE_DIR}/src/pyhpp_plot/__init__.py" + "${CMAKE_SOURCE_DIR}/src/pyhpp_plot/interactive_viewer.py" DESTINATION "${PYTHON_SITELIB}/pyhpp_plot") diff --git a/include/hpp/plot/hpp-native-graph.hh b/include/hpp/plot/hpp-native-graph.hh index 9407dd6..121b2f4 100644 --- a/include/hpp/plot/hpp-native-graph.hh +++ b/include/hpp/plot/hpp-native-graph.hh @@ -32,6 +32,7 @@ #ifndef HPP_PLOT_HPP_NATIVE_GRAPH_HH #define HPP_PLOT_HPP_NATIVE_GRAPH_HH +#include #include #include #include @@ -87,6 +88,25 @@ class HppNativeGraphWidget : public GraphWidget { /// \brief Display edge target constraints in the constraint panel void displayEdgeTargetConstraints(std::size_t id); + Q_SIGNALS: + /// \brief Emitted before showing node context menu, allows external code to + /// add actions + /// \param nodeId The ID of the node + /// \param nodeName The name of the node (state) + /// \param menu Pointer to the context menu (can add actions before it's + /// shown) + void nodeContextMenuAboutToShow(std::size_t nodeId, QString nodeName, + QMenu* menu); + + /// \brief Emitted before showing edge context menu, allows external code to + /// add actions + /// \param edgeId The ID of the edge + /// \param edgeName The name of the edge (transition) + /// \param menu Pointer to the context menu (can add actions before it's + /// shown) + void edgeContextMenuAboutToShow(std::size_t edgeId, QString edgeName, + QMenu* menu); + protected: /// \brief Fill scene from Graph object /// Reads nodes and edges directly from the C++ graph structure diff --git a/src/hpp-native-graph.cc b/src/hpp-native-graph.cc index 8d37721..bfce05b 100644 --- a/src/hpp-native-graph.cc +++ b/src/hpp-native-graph.cc @@ -433,6 +433,9 @@ void HppNativeGraphWidget::nodeContextMenu(QGVNode* node) { cm.addAction("Clear highlight", [this]() { highlightNode(-1); }); + // Allow external code (Python) to add custom actions + Q_EMIT nodeContextMenuAboutToShow(ni.id, ni.name, &cm); + cm.exec(QCursor::pos()); currentId_ = savedId; @@ -478,6 +481,9 @@ void HppNativeGraphWidget::edgeContextMenu(QGVEdge* edge) { cm.addAction("Clear highlight", [this]() { highlightEdge(-1); }); + // Allow external code (Python) to add custom actions + Q_EMIT edgeContextMenuAboutToShow(ei.id, ei.name, &cm); + cm.exec(QCursor::pos()); currentId_ = savedId; diff --git a/src/pyhpp_plot/__init__.py b/src/pyhpp_plot/__init__.py new file mode 100644 index 0000000..c5011a2 --- /dev/null +++ b/src/pyhpp_plot/__init__.py @@ -0,0 +1,21 @@ +from .graph_viewer import ( + MenuActionProxy, + show_graph, + show_graph_blocking, + show_interactive_graph, +) +from .interactive_viewer import ( + GraphViewerThread, + InteractiveGraphViewer, + show_interactive_graph_threaded, +) + +__all__ = [ + "GraphViewerThread", + "InteractiveGraphViewer", + "MenuActionProxy", + "show_graph", + "show_graph_blocking", + "show_interactive_graph", + "show_interactive_graph_threaded", +] diff --git a/src/pyhpp_plot/graph_viewer.cc b/src/pyhpp_plot/graph_viewer.cc index 8127591..a39854d 100644 --- a/src/pyhpp_plot/graph_viewer.cc +++ b/src/pyhpp_plot/graph_viewer.cc @@ -88,6 +88,29 @@ GraphPtr_t extractGraph(bp::object py_graph) { "'_get_native_graph()' method."); } +/// Simple wrapper to allow Python to add menu actions +class MenuActionProxy { + public: + MenuActionProxy(QMenu* menu) : menu_(menu) {} + + void addAction(const std::string& text, bp::object callback) { + QAction* action = menu_->addAction(QString::fromStdString(text)); + // Store callback and connect + QObject::connect(action, &QAction::triggered, [callback]() { + try { + callback(); + } catch (const bp::error_already_set&) { + PyErr_Print(); + } + }); + } + + void addSeparator() { menu_->addSeparator(); } + + private: + QMenu* menu_; +}; + /// This function blocks until the window is closed void showGraphBlocking(bp::object py_graph) { GraphPtr_t graph = extractGraph(py_graph); @@ -121,12 +144,102 @@ void showGraphBlocking(bp::object py_graph) { app.exec(); } +/// Interactive version with Python callbacks for context menus +void showInteractiveGraph(bp::object py_graph, bp::object node_callback, + bp::object edge_callback) { + GraphPtr_t graph = extractGraph(py_graph); + + if (!graph) { + throw std::runtime_error("Graph is null"); + } + + int argc = 1; + static char app_name[] = "hpp-plot-interactive"; + char* argv[] = {app_name, nullptr}; + + QApplication app(argc, argv); + + // Create main window + QMainWindow window; + window.setWindowTitle( + QString::fromStdString("Constraint Graph: " + graph->name())); + window.resize(1200, 800); + + // Create widget + hpp::plot::HppNativeGraphWidget widget(graph, &window); + widget.setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + window.setCentralWidget(&widget); + + // Connect Qt signals to Python callbacks + if (!node_callback.is_none()) { + QObject::connect( + &widget, &hpp::plot::HppNativeGraphWidget::nodeContextMenuAboutToShow, + [node_callback](std::size_t nodeId, QString nodeName, QMenu* menu) { + try { + MenuActionProxy proxy(menu); + node_callback(nodeId, nodeName.toStdString(), boost::ref(proxy)); + } catch (const bp::error_already_set&) { + PyErr_Print(); + } + }); + } + + if (!edge_callback.is_none()) { + QObject::connect( + &widget, &hpp::plot::HppNativeGraphWidget::edgeContextMenuAboutToShow, + [edge_callback](std::size_t edgeId, QString edgeName, QMenu* menu) { + try { + MenuActionProxy proxy(menu); + edge_callback(edgeId, edgeName.toStdString(), boost::ref(proxy)); + } catch (const bp::error_already_set&) { + PyErr_Print(); + } + }); + } + + // Show and refresh + window.show(); + widget.updateGraph(); + + // Run event loop (blocking) + app.exec(); +} + } // namespace BOOST_PYTHON_MODULE(graph_viewer) { + // Expose MenuActionProxy for adding menu actions from Python + bp::class_("MenuActionProxy", + bp::no_init) + .def("addAction", &MenuActionProxy::addAction, + (bp::arg("text"), bp::arg("callback")), + "Add an action to the context menu.\n\n" + "Args:\n" + " text: The text label for the menu action\n" + " callback: Python callable to invoke when action is triggered\n") + .def("addSeparator", &MenuActionProxy::addSeparator, + "Add a separator line to the context menu\n"); + bp::def("show_graph", &showGraphBlocking, bp::arg("graph"), "Show constraint graph in a Qt viewer (blocking).\n\n" "This function blocks until the viewer window is closed.\n\n" "Args:\n" " graph: The Graph object from pyhpp.manipulation\n"); + + // Alias for backwards compatibility + bp::def("show_graph_blocking", &showGraphBlocking, bp::arg("graph"), + "Alias for show_graph() for backwards compatibility.\n"); + + bp::def("show_interactive_graph", &showInteractiveGraph, + (bp::arg("graph"), bp::arg("node_callback") = bp::object(), + bp::arg("edge_callback") = bp::object()), + "Show constraint graph with interactive context menu callbacks.\n\n" + "This function blocks until the viewer window is closed.\n" + "Callbacks are invoked when user right-clicks on nodes/edges.\n\n" + "Args:\n" + " graph: The Graph object from pyhpp.manipulation\n" + " node_callback: Optional callback(node_id, node_name, " + "menu_proxy) for node menus\n" + " edge_callback: Optional callback(edge_id, edge_name, " + "menu_proxy) for edge menus\n"); } diff --git a/src/pyhpp_plot/interactive_viewer.py b/src/pyhpp_plot/interactive_viewer.py new file mode 100644 index 0000000..807a157 --- /dev/null +++ b/src/pyhpp_plot/interactive_viewer.py @@ -0,0 +1,273 @@ +"""Interactive constraint graph viewer with Python callbacks. + +This module provides a wrapper around the native graph viewer that allows +Python code to add custom actions to context menus for nodes and edges. +Implements all features from the CORBA hpp-monitoring-plugin. +""" + +import threading + + +class InteractiveGraphViewer: + """Wrapper for HppNativeGraphWidget with Python-based context menu actions.""" + + def __init__(self, graph, problem, config_callback=None): + """Initialize the interactive graph viewer. + + Args: + graph: PyWGraph from pyhpp.manipulation + problem: PyWProblem from pyhpp.manipulation + config_callback: Optional callable(config, label) that receives + generated configurations + """ + self.graph = graph + self.problem = problem + self.config_callback = config_callback or (lambda config, label: None) + self.current_config = None + + def show(self): + """Show graph viewer (blocking - runs Qt event loop until window closes).""" + from pyhpp_plot.graph_viewer import show_interactive_graph + + show_interactive_graph( + self.graph, + node_callback=self._on_node_context_menu, + edge_callback=self._on_edge_context_menu, + ) + + def _on_node_context_menu(self, node_id, node_name, menu): + """Add custom actions to node context menu. + + Args: + node_id: ID of the node (state) - from C++ graph + node_name: Name of the node (state) - from C++ graph + menu: MenuActionProxy for adding actions + """ + menu.addSeparator() + + menu.addAction( + "&Generate random config", lambda: self._generate_random_config(node_name) + ) + + menu.addAction( + "Generate from ¤t config", + lambda: self._generate_from_current_config(node_name), + ) + + menu.addAction( + "Set as &target state", lambda: self._set_target_state(node_name) + ) + + def _on_edge_context_menu(self, edge_id, edge_name, menu): + """Add custom actions to edge context menu. + + Args: + edge_id: ID of the edge (transition) - from C++ graph + edge_name: Name of the edge (transition) - from C++ graph + menu: MenuActionProxy for adding actions + """ + menu.addSeparator() + + menu.addAction( + "&Extend current config", lambda: self._extend_current_to_current(edge_name) + ) + + menu.addAction( + "&Extend current config to random config", + lambda: self._extend_current_to_random(edge_name), + ) + + def _generate_random_config(self, state_name): + """Generate random config and project to state. + + Args: + state_name: Name of the state + """ + try: + state = self.graph.getState(state_name) + shooter = self.problem.configurationShooter() + + min_error = float("inf") + for i in range(20): + q_random = shooter.shoot() + success, q_proj, error = self.graph.applyStateConstraints( + state, q_random + ) + + if success: + self.current_config = q_proj + self.config_callback( + q_proj, f"Random config in state: {state_name}" + ) + print(f"Generated config for state '{state_name}'") + return + + if error < min_error: + min_error = error + + print( + f"Failed to generate config for state '{state_name}' " + f"after 20 attempts (min error: {min_error:.6f})" + ) + except Exception as e: + print(f"Error generating random config: {e}") + import traceback + + traceback.print_exc() + + def _generate_from_current_config(self, state_name): + """Project current config to state. + + Args: + state_name: Name of the state + """ + try: + if self.current_config is None: + print("No current config available. Generate a config first.") + return + + state = self.graph.getState(state_name) + success, q_proj, error = self.graph.applyStateConstraints( + state, self.current_config + ) + + if success: + self.current_config = q_proj + self.config_callback( + q_proj, f"Current config projected to state: {state_name}" + ) + print(f"Projected current config to state '{state_name}'") + else: + print( + f"Failed to project current config to state " + f"'{state_name}' (error: {error:.6f})" + ) + except Exception as e: + print(f"Error projecting current config: {e}") + import traceback + + traceback.print_exc() + + def _set_target_state(self, state_name): + """Set state as goal for planning. + + Args: + state_name: Name of the state + """ + try: + state = self.graph.getState(state_name) + shooter = self.problem.configurationShooter() + + for i in range(20): + q_random = shooter.shoot() + success, q_goal, _error = self.graph.applyStateConstraints( + state, q_random + ) + + if success: + self.problem.addGoalConfig(q_goal) + print(f"Added goal config in state '{state_name}'") + return + + print(f"Failed to generate goal config for state '{state_name}'") + except Exception as e: + print(f"Error setting target state: {e}") + import traceback + + traceback.print_exc() + + def _extend_current_to_current(self, edge_name): + """Generate target config along edge from current config. + calls generateTargetConfig(edge, current, current). + + Args: + edge_name: Name of the edge + """ + try: + if self.current_config is None: + print("No current config available. Generate a config first.") + return + + edge = self.graph.getTransition(edge_name) + success, q_out, error = self.graph.generateTargetConfig( + edge, self.current_config, self.current_config + ) + + if success: + self.current_config = q_out + self.config_callback(q_out, f"Extended along edge: {edge_name}") + print(f"Extended current config along edge '{edge_name}'") + else: + print(f"Failed to extend along edge '{edge_name}' (error: {error:.6f})") + except Exception as e: + print(f"Error extending current config: {e}") + import traceback + + traceback.print_exc() + + def _extend_current_to_random(self, edge_name): + """Generate target config along edge to random config. + calls generateTargetConfig(edge, current, random). + + Args: + edge_name: Name of the edge + """ + try: + if self.current_config is None: + print("No current config available. Generate a config first.") + return + + edge = self.graph.getTransition(edge_name) + shooter = self.problem.configurationShooter() + q_random = shooter.shoot() + + success, q_out, error = self.graph.generateTargetConfig( + edge, self.current_config, q_random + ) + + if success: + self.current_config = q_out + self.config_callback( + q_out, f"Extended along edge to random: {edge_name}" + ) + print(f"Extended to random config along edge '{edge_name}'") + else: + print( + f"Failed to extend to random along edge '{edge_name}' " + f"(error: {error:.6f})" + ) + except Exception as e: + print(f"Error extending to random config: {e}") + import traceback + + traceback.print_exc() + + +class GraphViewerThread(threading.Thread): + """Runs graph viewer in separate daemon thread with Qt event loop.""" + + def __init__(self, graph, problem, config_callback): + super().__init__(daemon=True, name="GraphViewerThread") + self.graph = graph + self.problem = problem + self.config_callback = config_callback + + def run(self): + viewer = InteractiveGraphViewer(self.graph, self.problem, self.config_callback) + viewer.show() + + +def show_interactive_graph_threaded(graph, problem, config_callback): + """Show interactive graph viewer in a separate thread (non-blocking). + + Args: + graph: PyWGraph from pyhpp.manipulation + problem: PyWProblem from pyhpp.manipulation + config_callback: Callable(config, label) called when configs are generated + + Returns: + GraphViewerThread object (already started) + """ + thread = GraphViewerThread(graph, problem, config_callback) + thread.start() + return thread