WaypointFollowingVisualization.cpp
Go to the documentation of this file.
2 
3 namespace rsm {
4 
6  _waypoint_server("waypoint_marker") {
7  ros::NodeHandle nh("rsm");
8  _waypoint_subscriber = nh.subscribe<rsm_msgs::WaypointArray>(
10  this);
11  _move_waypoint_client = nh.serviceClient<rsm_msgs::MoveWaypoint>(
12  "moveWaypoint");
14  nh.serviceClient<rsm_msgs::RemoveWaypoint>(
15  "removeWaypoint");
17  rsm_msgs::SetWaypointRoutine>("setWaypointRoutine");
19  rsm_msgs::GetWaypointRoutines>("getWaypointRoutines");
20  _menu_handler.insert("Delete",
22  _1));
27  this, false);
29 }
30 
32 }
33 
35  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
36  rsm_msgs::MoveWaypoint srv;
37  srv.request.position = std::stoi(feedback->marker_name);
38  srv.request.pose = feedback->pose;
39  if (_move_waypoint_client.call(srv)) {
40  //ROS_INFO("Successful call to service Move Waypoint");
41  } else {
42  ROS_ERROR("Failed to call service Move Waypoint");
43  }
44 }
45 
47  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
48  rsm_msgs::RemoveWaypoint srv;
49  srv.request.position = std::stoi(feedback->marker_name);
50  if (_remove_waypoint_client.call(srv)) {
51  //ROS_INFO("Successful call to service Remove Waypoint");
52  } else {
53  ROS_ERROR("Failed to call service Remove Waypoint");
54  }
55 }
56 
58  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) {
59  rsm_msgs::SetWaypointRoutine srv;
60  srv.request.position = std::stoi(feedback->marker_name);
61  if (feedback->menu_entry_id == 3) { //Set to none
62  srv.request.routine = "";
63  } else {
64  srv.request.routine = _waypoint_routines[feedback->menu_entry_id - 4]; //three previous menu entries
65  }
67  ROS_ERROR("Failed to call service Move Waypoint");
68  }
69 
70 }
71 
73  const rsm_msgs::WaypointArray::ConstPtr& waypoint_array) {
74  if (_refresh_waypoint_markers || waypointArrayChanged(waypoint_array)) {
75  _waypoints = *waypoint_array;
77  for (int i = 0; i < _waypoints.waypoints_size; i++) {
79  }
82  }
83 }
84 
86  int waypoint_pos) {
87  visualization_msgs::InteractiveMarker waypoint_marker;
88  waypoint_marker.header.frame_id = "map";
89  waypoint_marker.name = std::to_string(waypoint_pos);
90  if (!_waypoints.waypoints[waypoint_pos].routine.empty()) {
91  std::ostringstream s;
92  s << waypoint_marker.name << ": "
93  << _waypoints.waypoints[waypoint_pos].routine;
94  waypoint_marker.description = s.str();
95  } else {
96  waypoint_marker.description = waypoint_marker.name;
97  }
98  waypoint_marker.pose = _waypoints.waypoints[waypoint_pos].pose;
99 
100 // create a colorized flag pole
101  visualization_msgs::Marker box_marker;
102  box_marker.type = visualization_msgs::Marker::MESH_RESOURCE;
103  box_marker.mesh_resource =
104  "package://rsm_rviz_plugins/media/flag.dae";
105  box_marker.mesh_use_embedded_materials = false;
106  box_marker.scale.x = 1.0;
107  box_marker.scale.y = 1.0;
108  box_marker.scale.z = 1.0;
109  if (_waypoints.waypoints[waypoint_pos].unreachable) {
110  box_marker.color.r = 1.0;
111  } else if (_waypoints.waypoints[waypoint_pos].visited) {
112  box_marker.color.g = 1.0;
113  } else {
114  box_marker.color.b = 1.0;
115  }
116  box_marker.color.a = 1.0;
117 
118 // create a non-interactive control which contains the box
119  visualization_msgs::InteractiveMarkerControl box_control;
120  box_control.always_visible = true;
121  box_control.markers.push_back(box_marker);
122 
123 // add the control to the interactive marker
124  waypoint_marker.controls.push_back(box_control);
125 
126  visualization_msgs::InteractiveMarkerControl menu_control;
127  menu_control.name = "menu_control";
128  menu_control.interaction_mode =
129  visualization_msgs::InteractiveMarkerControl::MENU;
130  menu_control.always_visible = true;
131  menu_control.markers.push_back(box_marker);
132  waypoint_marker.controls.push_back(menu_control);
133 
134 // create a control which will move the box
135 // this control does not contain any markers,
136 // which will cause RViz to insert two arrows
137  visualization_msgs::InteractiveMarkerControl move_control;
138  move_control.name = "move_control";
139  tf::Quaternion orientation(0.0, 1.0, 0.0, 1.0);
140  orientation.normalize();
141  tf::quaternionTFToMsg(orientation, move_control.orientation);
142  move_control.interaction_mode =
143  visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE;
144  waypoint_marker.controls.push_back(move_control);
145  move_control.interaction_mode =
146  visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
147  waypoint_marker.controls.push_back(move_control);
148 
149 // add the interactive marker to our collection &
150 // tell the _waypointServer to call processFeedback() when feedback arrives for it
151  _waypoint_server.insert(waypoint_marker);
152  _waypoint_server.setCallback(waypoint_marker.name,
154  _1));
155  _menu_handler.apply(_waypoint_server, waypoint_marker.name);
156 }
157 
159  const ros::TimerEvent& event) {
161  _menu_handler.insert("Set routine");
162  _menu_handler.insert(sub_menu_handle, "None",
164  this, _1));
165  rsm_msgs::GetWaypointRoutines srv;
167  _waypoint_routines = srv.response.waypointRoutines;
168  for (auto it : _waypoint_routines) {
169  _menu_handler.insert(sub_menu_handle, it,
170  boost::bind(
172  this, _1));
173  }
174  } else {
175  ROS_ERROR("Failed to call Get Waypoint Routines service");
176  }
177 }
178 
180  const rsm_msgs::WaypointArray::ConstPtr& waypoint_array) {
181  if (waypoint_array->waypoints_size == _waypoints.waypoints_size) {
182  for (int i = 0; i < _waypoints.waypoints_size; i++) {
183  if (_waypoints.waypoints[i].routine.compare(
184  waypoint_array->waypoints[i].routine) != 0
185  || _waypoints.waypoints[i].visited
186  != waypoint_array->waypoints[i].visited
187  || _waypoints.waypoints[i].unreachable
188  != waypoint_array->waypoints[i].unreachable) {
189  return true;
190  }
191  }
192  return false;
193  } else {
194  return true;
195  }
196 }
197 
199  const ros::TimerEvent& event) {
201 }
202 
203 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
void setWaypointRoutine(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
interactive_markers::InteractiveMarkerServer _waypoint_server
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool waypointArrayChanged(const rsm_msgs::WaypointArray::ConstPtr &waypoint_array)
XmlRpcServer s
bool call(MReq &req, MRes &res)
Quaternion & normalize()
bool apply(InteractiveMarkerServer &server, const std::string &marker_name)
void timerCallback(const ros::TimerEvent &event)
interactive_markers::MenuHandler _menu_handler
void periodicalRefreshTimerCallback(const ros::TimerEvent &event)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void waypointCallback(const rsm_msgs::WaypointArray::ConstPtr &waypoint_array)
EntryHandle insert(const std::string &title, const FeedbackCallback &feedback_cb)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
INTERACTIVE_MARKERS_PUBLIC bool setCallback(const std::string &name, FeedbackCallback feedback_cb, uint8_t feedback_type=DEFAULT_FEEDBACK_CB)
void removeWaypoint(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
INTERACTIVE_MARKERS_PUBLIC void clear()
#define ROS_ERROR(...)


rsm_rviz_plugins
Author(s): Marco Steinbrink
autogenerated on Tue Mar 16 2021 02:44:40