loop_closure_assistant.cpp
Go to the documentation of this file.
1 /*
2  * loop_closure_assistant
3  * Copyright (c) 2019, Samsung Research America
4  *
5  * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
6  * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
7  * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
8  * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
9  *
10  * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
11  * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
12  * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
13  * CONDITIONS.
14  *
15  */
16 
17 /* Author: Steven Macenski */
18 
20 
21 namespace loop_closure_assistant
22 {
23 
24 /*****************************************************************************/
26  ros::NodeHandle& node,
27  karto::Mapper* mapper,
28  laser_utils::ScanHolder* scan_holder,
29  PausedState& state, ProcessType & processor_type)
30 : mapper_(mapper), scan_holder_(scan_holder),
31  interactive_mode_(false), nh_(node), state_(state),
32  processor_type_(processor_type)
33 /*****************************************************************************/
34 {
35  node.setParam("paused_processing", false);
36  tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
37  ssClear_manual_ = node.advertiseService("clear_changes",
39  ssLoopClosure_ = node.advertiseService("manual_loop_closure",
41  scan_publisher_ = node.advertise<sensor_msgs::LaserScan>(
42  "karto_scan_visualization",10);
45  std::make_unique<interactive_markers::InteractiveMarkerServer>(
46  "slam_toolbox","",true);
47  ssInteractive_ = node.advertiseService("toggle_interactive_mode",
49  node.setParam("interactive_mode", interactive_mode_);
50  marker_publisher_ = node.advertise<visualization_msgs::MarkerArray>(
51  "karto_graph_visualization",1);
52  node.param("map_frame", map_frame_, std::string("map"));
53  node.param("enable_interactive_mode", enable_interactive_mode_, false);
54 }
55 
56 /*****************************************************************************/
58  visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
59 /*****************************************************************************/
60 {
61  if (processor_type_ != PROCESS)
62  {
64  "Interactive mode is invalid outside processing mode.");
65  return;
66  }
67 
68  const int id = std::stoi(feedback->marker_name, nullptr, 10) - 1;
69 
70  // was depressed, something moved, and now released
71  if (feedback->event_type ==
72  visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP &&
73  feedback->mouse_point_valid)
74  {
75  addMovedNodes(id, Eigen::Vector3d(feedback->mouse_point.x,
76  feedback->mouse_point.y, tf2::getYaw(feedback->pose.orientation)));
77  }
78 
79  // is currently depressed, being moved before release
80  if (feedback->event_type ==
81  visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
82  {
83  // get scan
84  sensor_msgs::LaserScan scan = scan_holder_->getCorrectedScan(id);
85 
86  // get correct orientation
87  tf2::Quaternion quat(0.,0.,0.,1.0), msg_quat(0.,0.,0.,1.0);
88  double node_yaw, first_node_yaw;
89  solver_->GetNodeOrientation(id, node_yaw);
90  solver_->GetNodeOrientation(0, first_node_yaw);
91  tf2::Quaternion q1(0.,0.,0.,1.0);
92  q1.setEuler(0., 0., node_yaw - 3.14159);
93  tf2::Quaternion q2(0.,0.,0.,1.0);
94  q2.setEuler(0., 0., 3.14159);
95  quat *= q1;
96  quat *= q2;
97 
98  // interactive move
99  tf2::convert(feedback->pose.orientation, msg_quat);
100  quat *= msg_quat;
101  quat.normalize();
102 
103  // create correct transform
104  tf2::Transform transform;
105  transform.setOrigin(tf2::Vector3(feedback->pose.position.x,
106  feedback->pose.position.y, 0.));
107  transform.setRotation(quat);
108 
109  // publish the scan visualization with transform
110  geometry_msgs::TransformStamped msg;
111  tf2::convert(transform, msg.transform);
112  msg.child_frame_id = "karto_scan_visualization";
113  msg.header.frame_id = feedback->header.frame_id;
114  msg.header.stamp = ros::Time::now();
115  tfB_->sendTransform(msg);
116 
117  scan.header.frame_id = "karto_scan_visualization";
118  scan.header.stamp = ros::Time::now();
119  scan_publisher_.publish(scan);
120  }
121 }
122 
123 /*****************************************************************************/
125 /*****************************************************************************/
126 {
127  interactive_server_->clear();
128  std::unordered_map<int, Eigen::Vector3d>* graph = solver_->getGraph();
129 
130  if (graph->size() == 0)
131  {
132  return;
133  }
134 
135  ROS_DEBUG("Graph size: %i",(int)graph->size());
136  bool interactive_mode = false;
137  {
138  boost::mutex::scoped_lock lock(interactive_mutex_);
139  interactive_mode = interactive_mode_;
140  }
141 
142  visualization_msgs::MarkerArray marray;
143  visualization_msgs::Marker m = vis_utils::toMarker(map_frame_,
144  "slam_toolbox", 0.1);
145 
146  for (ConstGraphIterator it = graph->begin(); it != graph->end(); ++it)
147  {
148  m.id = it->first + 1;
149  m.pose.position.x = it->second(0);
150  m.pose.position.y = it->second(1);
151 
152  if (interactive_mode && enable_interactive_mode_)
153  {
154  visualization_msgs::InteractiveMarker int_marker =
156  interactive_server_->insert(int_marker,
157  boost::bind(
159  this, _1));
160  }
161  else
162  {
163  marray.markers.push_back(m);
164  }
165  }
166 
167  // if disabled, clears out old markers
168  interactive_server_->applyChanges();
169  marker_publisher_.publish(marray);
170  return;
171 }
172 
173 /*****************************************************************************/
175  slam_toolbox_msgs::LoopClosure::Request& req,
176  slam_toolbox_msgs::LoopClosure::Response& resp)
177 /*****************************************************************************/
178 {
180  {
181  ROS_WARN("Called manual loop closure"
182  " with interactive mode disabled. Ignoring.");
183  return false;
184  }
185 
186  {
187  boost::mutex::scoped_lock lock(moved_nodes_mutex_);
188 
189  if (moved_nodes_.size() == 0)
190  {
191  ROS_WARN("No moved nodes to attempt manual loop closure.");
192  return true;
193  }
194 
195  ROS_INFO("LoopClosureAssistant: Attempting to manual "
196  "loop close with %i moved nodes.", (int)moved_nodes_.size());
197  // for each in node map
198  std::map<int, Eigen::Vector3d>::const_iterator it = moved_nodes_.begin();
199  for (it; it != moved_nodes_.end(); ++it)
200  {
201  moveNode(it->first,
202  Eigen::Vector3d(it->second(0),it->second(1), it->second(2)));
203  }
204  }
205 
206  // optimize
208 
209  // update visualization and clear out nodes completed
210  publishGraph();
211  clearMovedNodes();
212  return true;
213 }
214 
215 /*****************************************************************************/
217  slam_toolbox_msgs::ToggleInteractive::Request &req,
218  slam_toolbox_msgs::ToggleInteractive::Response &resp)
219 /*****************************************************************************/
220 {
222  {
223  ROS_WARN("Called toggle interactive mode with "
224  "interactive mode disabled. Ignoring.");
225  return false;
226  }
227 
228  bool interactive_mode;
229  {
230  boost::mutex::scoped_lock lock_i(interactive_mutex_);
232  interactive_mode = interactive_mode_;
233  nh_.setParam("interactive_mode", interactive_mode_);
234  }
235 
236  ROS_INFO("SlamToolbox: Toggling %s interactive mode.",
237  interactive_mode ? "on" : "off");
238  publishGraph();
239  clearMovedNodes();
240 
241  // set state so we don't overwrite changes in rviz while loop closing
242  state_.set(PROCESSING, interactive_mode);
243  state_.set(VISUALIZING_GRAPH, interactive_mode);
244  nh_.setParam("paused_processing", interactive_mode);
245  return true;
246 }
247 
248 /*****************************************************************************/
250  const int& id, const Eigen::Vector3d& pose)
251 /*****************************************************************************/
252 {
253  solver_->ModifyNode(id, pose);
254 }
255 
256 /*****************************************************************************/
258  slam_toolbox_msgs::Clear::Request& req,
259  slam_toolbox_msgs::Clear::Response& resp)
260 /*****************************************************************************/
261 {
263  {
264  ROS_WARN("Called Clear changes with interactive mode disabled. Ignoring.");
265  return false;
266  }
267 
268  ROS_INFO("LoopClosureAssistant: Clearing manual loop closure nodes.");
269  publishGraph();
270  clearMovedNodes();
271  return true;
272 }
273 
274 /*****************************************************************************/
276 /*****************************************************************************/
277 {
278  boost::mutex::scoped_lock lock(moved_nodes_mutex_);
279  moved_nodes_.clear();
280 }
281 
282 /*****************************************************************************/
283 void LoopClosureAssistant::addMovedNodes(const int& id, Eigen::Vector3d vec)
284 /*****************************************************************************/
285 {
286  ROS_INFO("LoopClosureAssistant: Node %i new manual loop closure "
287  "pose has been recorded.",id);
288  boost::mutex::scoped_lock lock(moved_nodes_mutex_);
289  moved_nodes_[id] = vec;
290 }
291 
292 } // end namespace
virtual void GetNodeOrientation(const int &unique_id, double &pose)
Definition: Mapper.h:1035
void moveNode(const int &id, const Eigen::Vector3d &pose)
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
LoopClosureAssistant(ros::NodeHandle &node, karto::Mapper *mapper, laser_utils::ScanHolder *scan_holder, PausedState &state, ProcessType &processor_type)
void addMovedNodes(const int &id, Eigen::Vector3d vec)
bool interactiveModeCallback(slam_toolbox_msgs::ToggleInteractive::Request &req, slam_toolbox_msgs::ToggleInteractive::Response &resp)
sensor_msgs::LaserScan getCorrectedScan(const int &id)
std::unique_ptr< interactive_markers::InteractiveMarkerServer > interactive_server_
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
Definition: Mapper.h:1020
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std::unordered_map< int, Eigen::Vector3d >::const_iterator ConstGraphIterator
#define ROS_WARN(...)
virtual void ModifyNode(const int &unique_id, Eigen::Vector3d pose)
Definition: Mapper.h:1028
Quaternion & normalize()
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void CorrectPoses()
Definition: Mapper.h:2036
#define ROS_INFO(...)
#define ROS_ERROR_THROTTLE(period,...)
ScanSolver * getScanSolver()
Definition: Mapper.cpp:3229
bool manualLoopClosureCallback(slam_toolbox_msgs::LoopClosure::Request &req, slam_toolbox_msgs::LoopClosure::Response &resp)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void setEuler(const tf2Scalar &yaw, const tf2Scalar &pitch, const tf2Scalar &roll)
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
double getYaw(const A &a)
visualization_msgs::Marker toMarker(const std::string &frame, const std::string &ns, const double &scale)
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
visualization_msgs::InteractiveMarker toInteractiveMarker(visualization_msgs::Marker &marker, const double &scale)
static Time now()
void convert(const A &a, B &b)
bool clearChangesCallback(slam_toolbox_msgs::Clear::Request &req, slam_toolbox_msgs::Clear::Response &resp)
#define ROS_DEBUG(...)


slam_toolbox
Author(s): Steve Macenski
autogenerated on Wed Mar 3 2021 03:49:20