30 : mapper_(mapper), scan_holder_(scan_holder),
31 interactive_mode_(false), nh_(node), state_(state),
32 processor_type_(processor_type)
35 node.
setParam(
"paused_processing",
false);
36 tfB_ = std::make_unique<tf2_ros::TransformBroadcaster>();
42 "karto_scan_visualization",10);
45 std::make_unique<interactive_markers::InteractiveMarkerServer>(
46 "slam_toolbox",
"",
true);
51 "karto_graph_visualization",1);
58 visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
64 "Interactive mode is invalid outside processing mode.");
68 const int id = std::stoi(feedback->marker_name,
nullptr, 10) - 1;
71 if (feedback->event_type ==
72 visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP &&
73 feedback->mouse_point_valid)
76 feedback->mouse_point.y,
tf2::getYaw(feedback->pose.orientation)));
80 if (feedback->event_type ==
81 visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE)
88 double node_yaw, first_node_yaw;
92 q1.
setEuler(0., 0., node_yaw - 3.14159);
105 transform.
setOrigin(tf2::Vector3(feedback->pose.position.x,
106 feedback->pose.position.y, 0.));
110 geometry_msgs::TransformStamped msg;
112 msg.child_frame_id =
"karto_scan_visualization";
113 msg.header.frame_id = feedback->header.frame_id;
115 tfB_->sendTransform(msg);
117 scan.header.frame_id =
"karto_scan_visualization";
130 if (graph->size() == 0)
135 ROS_DEBUG(
"Graph size: %i",(
int)graph->size());
136 bool interactive_mode =
false;
142 visualization_msgs::MarkerArray marray;
144 "slam_toolbox", 0.1);
148 m.id = it->first + 1;
149 m.pose.position.x = it->second(0);
150 m.pose.position.y = it->second(1);
154 visualization_msgs::InteractiveMarker int_marker =
163 marray.markers.push_back(m);
175 slam_toolbox_msgs::LoopClosure::Request& req,
176 slam_toolbox_msgs::LoopClosure::Response& resp)
181 ROS_WARN(
"Called manual loop closure" 182 " with interactive mode disabled. Ignoring.");
191 ROS_WARN(
"No moved nodes to attempt manual loop closure.");
195 ROS_INFO(
"LoopClosureAssistant: Attempting to manual " 196 "loop close with %i moved nodes.", (
int)
moved_nodes_.size());
198 std::map<int, Eigen::Vector3d>::const_iterator it =
moved_nodes_.begin();
202 Eigen::Vector3d(it->second(0),it->second(1), it->second(2)));
217 slam_toolbox_msgs::ToggleInteractive::Request &req,
218 slam_toolbox_msgs::ToggleInteractive::Response &resp)
223 ROS_WARN(
"Called toggle interactive mode with " 224 "interactive mode disabled. Ignoring.");
228 bool interactive_mode;
236 ROS_INFO(
"SlamToolbox: Toggling %s interactive mode.",
237 interactive_mode ?
"on" :
"off");
244 nh_.
setParam(
"paused_processing", interactive_mode);
250 const int&
id,
const Eigen::Vector3d& pose)
258 slam_toolbox_msgs::Clear::Request& req,
259 slam_toolbox_msgs::Clear::Response& resp)
264 ROS_WARN(
"Called Clear changes with interactive mode disabled. Ignoring.");
268 ROS_INFO(
"LoopClosureAssistant: Clearing manual loop closure nodes.");
286 ROS_INFO(
"LoopClosureAssistant: Node %i new manual loop closure " 287 "pose has been recorded.",
id);
virtual void GetNodeOrientation(const int &unique_id, double &pose)
void moveNode(const int &id, const Eigen::Vector3d &pose)
ros::ServiceServer ssLoopClosure_
std::unique_ptr< tf2_ros::TransformBroadcaster > tfB_
LoopClosureAssistant(ros::NodeHandle &node, karto::Mapper *mapper, laser_utils::ScanHolder *scan_holder, PausedState &state, ProcessType &processor_type)
boost::mutex interactive_mutex_
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_
boost::mutex moved_nodes_mutex_
virtual std::unordered_map< int, Eigen::Vector3d > * getGraph()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
laser_utils::ScanHolder * scan_holder_
virtual void ModifyNode(const int &unique_id, Eigen::Vector3d pose)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void publish(const boost::shared_ptr< M > &message) const
void processInteractiveFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
#define ROS_ERROR_THROTTLE(period,...)
ros::ServiceServer ssInteractive_
bool enable_interactive_mode_
ScanSolver * getScanSolver()
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)
karto::ScanSolver * solver_
double getYaw(const A &a)
ProcessType & processor_type_
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)
ros::ServiceServer ssClear_manual_
void convert(const A &a, B &b)
std::map< int, Eigen::Vector3d > moved_nodes_
ros::Publisher marker_publisher_
ros::Publisher scan_publisher_
bool clearChangesCallback(slam_toolbox_msgs::Clear::Request &req, slam_toolbox_msgs::Clear::Response &resp)