diff --git a/mav_planning_rviz/CMakeLists.txt b/mav_planning_rviz/CMakeLists.txt index bc1bbd5b..4cad4869 100644 --- a/mav_planning_rviz/CMakeLists.txt +++ b/mav_planning_rviz/CMakeLists.txt @@ -20,7 +20,6 @@ set(SRC_FILES src/planning_panel.cpp src/pose_widget.cpp src/edit_button.cpp - src/planning_interactive_markers.cpp src/goal_marker.cpp ) diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h b/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h deleted file mode 100644 index 5fc49cf3..00000000 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_interactive_markers.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef MAV_PLANNING_RVIZ_PLANNING_INTERACTIVE_MARKERS_H_ -#define MAV_PLANNING_RVIZ_PLANNING_INTERACTIVE_MARKERS_H_ - -#include - -#include -#include -#include -#include - -namespace mav_planning_rviz { - -class PlanningInteractiveMarkers { - public: - typedef std::function PoseUpdatedFunctionType; - - PlanningInteractiveMarkers(const ros::NodeHandle& nh); - ~PlanningInteractiveMarkers() {} - - void setFrameId(const std::string& frame_id); - // Bind callback for whenever pose updates. - - void setPoseUpdatedCallback(const PoseUpdatedFunctionType& function) { pose_updated_function_ = function; } - - void initialize(); - - // Functions to interface with the set_pose marker: - void enableSetPoseMarker(const mav_msgs::EigenTrajectoryPoint& pose); - void disableSetPoseMarker(); - void setPose(const mav_msgs::EigenTrajectoryPoint& pose); - - // Functions to interact with markers from the marker map (no controls): - void enableMarker(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose); - void updateMarkerPose(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose); - void disableMarker(const std::string& id); - - void processSetPoseFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); - - private: - // Creates markers without adding them to the marker server. - void createMarkers(); - - // ROS stuff. - ros::NodeHandle nh_; - interactive_markers::InteractiveMarkerServer marker_server_; - - // Settings. - std::string frame_id_; - - // State. - bool initialized_; - visualization_msgs::InteractiveMarker set_pose_marker_; - - // This is map for waypoint visualization markers: - std::map marker_map_; - // This determines how the markers in the marker map will look: - visualization_msgs::InteractiveMarker marker_prototype_; - - // State: - PoseUpdatedFunctionType pose_updated_function_; -}; - -} // end namespace mav_planning_rviz - -#endif // MAV_PLANNING_RVIZ_PLANNING_INTERACTIVE_MARKERS_H_ diff --git a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h index 50c743b0..71e45b26 100644 --- a/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h +++ b/mav_planning_rviz/include/mav_planning_rviz/planning_panel.h @@ -8,9 +8,10 @@ #include #include "mav_planning_rviz/edit_button.h" #include "mav_planning_rviz/goal_marker.h" -#include "mav_planning_rviz/planning_interactive_markers.h" #include "mav_planning_rviz/pose_widget.h" #include "planner_msgs/NavigationStatus.h" +#include +#include #endif enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 }; @@ -123,8 +124,6 @@ class PlanningPanel : public rviz::Panel { // Keep track of all the pose <-> button widgets as they're related: std::map pose_widget_map_; std::map edit_button_map_; - // ROS state: - PlanningInteractiveMarkers interactive_markers_; // QT state: QString namespace_; diff --git a/mav_planning_rviz/src/planning_interactive_markers.cpp b/mav_planning_rviz/src/planning_interactive_markers.cpp deleted file mode 100644 index 3821f511..00000000 --- a/mav_planning_rviz/src/planning_interactive_markers.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "mav_planning_rviz/planning_interactive_markers.h" - -namespace mav_planning_rviz { - -PlanningInteractiveMarkers::PlanningInteractiveMarkers(const ros::NodeHandle& nh) - : nh_(nh), marker_server_("planning_markers"), frame_id_("odom"), initialized_(false) {} - -void PlanningInteractiveMarkers::setFrameId(const std::string& frame_id) { - frame_id_ = frame_id; - set_pose_marker_.header.frame_id = frame_id_; - marker_prototype_.header.frame_id = frame_id_; -} - -void PlanningInteractiveMarkers::initialize() { - createMarkers(); - initialized_ = true; -} - -void PlanningInteractiveMarkers::createMarkers() { - constexpr double kSqrt2Over2 = sqrt(2.0) / 2.0; - - // Set up controls: x, y, z, and yaw. - visualization_msgs::InteractiveMarkerControl control; - set_pose_marker_.controls.clear(); - control.orientation.w = kSqrt2Over2; - control.orientation.x = 0; - control.orientation.y = kSqrt2Over2; - control.orientation.z = 0; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; - control.name = "move plane"; - set_pose_marker_.controls.push_back(control); - - // Create a marker prototype, as the default style for all markers in the - // marker map: - marker_prototype_.header.frame_id = frame_id_; - marker_prototype_.scale = 1.0; - control.markers.clear(); - control.name = "arrow"; - control.interaction_mode = visualization_msgs::InteractiveMarkerControl::NONE; - visualization_msgs::Marker default_marker; - default_marker.type = visualization_msgs::Marker::ARROW; - default_marker.scale.x = 0.75; - default_marker.scale.y = 0.25; - default_marker.scale.z = 0.25; - control.markers.push_back(default_marker); - visualization_msgs::Marker text_marker; - text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; - text_marker.scale.z = 0.5; - text_marker.pose.position.z = 0.5; - text_marker.text = "placeholder"; - text_marker.id = 1; - control.markers.push_back(text_marker); - - marker_prototype_.controls.push_back(control); -} - -void PlanningInteractiveMarkers::enableSetPoseMarker(const mav_msgs::EigenTrajectoryPoint& pose) { - geometry_msgs::PoseStamped pose_stamped; - mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); - set_pose_marker_.pose = pose_stamped.pose; - - marker_server_.insert(set_pose_marker_); - marker_server_.setCallback(set_pose_marker_.name, - boost::bind(&PlanningInteractiveMarkers::processSetPoseFeedback, this, _1)); - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::disableSetPoseMarker() { - marker_server_.erase(set_pose_marker_.name); - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::setPose(const mav_msgs::EigenTrajectoryPoint& pose) { - geometry_msgs::PoseStamped pose_stamped; - mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); - set_pose_marker_.pose = pose_stamped.pose; - marker_server_.setPose(set_pose_marker_.name, set_pose_marker_.pose); - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::processSetPoseFeedback( - const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) { - if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) { - if (pose_updated_function_) { - mav_msgs::EigenTrajectoryPoint pose; - mav_msgs::eigenTrajectoryPointFromPoseMsg(feedback->pose, &pose); - pose_updated_function_(pose); - } - } - - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::enableMarker(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose) { - geometry_msgs::PoseStamped pose_stamped; - mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); - - auto search = marker_map_.find(id); - if (search != marker_map_.end()) { - // Already exists, just update the pose and enable it. - search->second.pose = pose_stamped.pose; - marker_server_.insert(search->second); - marker_server_.applyChanges(); - return; - } - - // Doesn't exist yet... Have to create it from prototype. - marker_map_[id] = marker_prototype_; - marker_map_[id].name = id; - marker_map_[id].controls[0].markers[1].text = id; - marker_map_[id].pose = pose_stamped.pose; - marker_server_.insert(marker_map_[id]); - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::updateMarkerPose(const std::string& id, const mav_msgs::EigenTrajectoryPoint& pose) { - auto search = marker_map_.find(id); - if (search == marker_map_.end()) { - return; - } - - geometry_msgs::PoseStamped pose_stamped; - mav_msgs::msgPoseStampedFromEigenTrajectoryPoint(pose, &pose_stamped); - search->second.pose = pose_stamped.pose; - marker_server_.setPose(id, pose_stamped.pose); - marker_server_.applyChanges(); -} - -void PlanningInteractiveMarkers::disableMarker(const std::string& id) { - marker_server_.erase(id); - marker_server_.applyChanges(); -} - -} // end namespace mav_planning_rviz diff --git a/mav_planning_rviz/src/planning_panel.cpp b/mav_planning_rviz/src/planning_panel.cpp index 2bd232ac..9872a1b8 100644 --- a/mav_planning_rviz/src/planning_panel.cpp +++ b/mav_planning_rviz/src/planning_panel.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include "mav_planning_rviz/edit_button.h" #include "mav_planning_rviz/goal_marker.h" @@ -31,7 +32,7 @@ namespace mav_planning_rviz { -PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent), nh_(ros::NodeHandle()), interactive_markers_(nh_) { +PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent), nh_(ros::NodeHandle()) { createLayout(); goal_marker_ = std::make_shared(nh_); planner_state_sub_ = nh_.subscribe("/planner_status", 1, &PlanningPanel::plannerstateCallback, this, @@ -39,17 +40,6 @@ PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent), nh_(ros::No } void PlanningPanel::onInitialize() { - interactive_markers_.initialize(); - interactive_markers_.setPoseUpdatedCallback( - std::bind(&PlanningPanel::updateInteractiveMarkerPose, this, std::placeholders::_1)); - - interactive_markers_.setFrameId(vis_manager_->getFixedFrame().toStdString()); - // Initialize all the markers. - for (const auto& kv : pose_widget_map_) { - mav_msgs::EigenTrajectoryPoint pose; - kv.second->getPose(&pose); - interactive_markers_.enableMarker(kv.first, pose); - } } void PlanningPanel::createLayout() { @@ -260,18 +250,11 @@ void PlanningPanel::startEditing(const std::string& id) { if (search == pose_widget_map_.end()) { return; } - // Update fixed frame (may have changed since last time): - interactive_markers_.setFrameId(vis_manager_->getFixedFrame().toStdString()); - mav_msgs::EigenTrajectoryPoint pose; - search->second->getPose(&pose); - interactive_markers_.enableSetPoseMarker(pose); - interactive_markers_.disableMarker(id); } void PlanningPanel::finishEditing(const std::string& id) { if (currently_editing_ == id) { currently_editing_.clear(); - interactive_markers_.disableSetPoseMarker(); } auto search = pose_widget_map_.find(id); if (search == pose_widget_map_.end()) { @@ -280,7 +263,6 @@ void PlanningPanel::finishEditing(const std::string& id) { ros::spinOnce(); mav_msgs::EigenTrajectoryPoint pose; search->second->getPose(&pose); - interactive_markers_.enableMarker(id, pose); } void PlanningPanel::registerPoseWidget(PoseWidget* widget) { @@ -331,10 +313,6 @@ void PlanningPanel::updateInteractiveMarkerPose(const mav_msgs::EigenTrajectoryP } void PlanningPanel::widgetPoseUpdated(const std::string& id, mav_msgs::EigenTrajectoryPoint& pose) { - if (currently_editing_ == id) { - interactive_markers_.setPose(pose); - } - interactive_markers_.updateMarkerPose(id, pose); } void PlanningPanel::callPlannerService() { @@ -604,7 +582,6 @@ void PlanningPanel::odometryCallback(const nav_msgs::Odometry& msg) { point.position_W = odometry.position_W; point.orientation_W_B = odometry.orientation_W_B; pose_widget_map_["start"]->setPose(point); - interactive_markers_.updateMarkerPose("start", point); } }