diff --git a/README.md b/README.md index f374ca3..9cdbe91 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,11 @@ [![Build Status](https://travis-ci.org/mrpt-ros-pkg/mrpt_slam.svg?branch=master)](https://travis-ci.org/mrpt-ros-pkg/mrpt_slam) -MRPT-based SLAM packages +MRPT-based SLAM packages ------------------------- -ROS wrappers for SLAM algorithms in Mobile Robot Programming Toolkit (MRPT). +ROS wrappers for SLAM algorithms in Mobile Robot Programming Toolkit (MRPT). Refer to http://wiki.ros.org/mrpt_slam for further documentation. -Branches: +Branches: * `master`: Intended for use with latest MRPT versions (1.5.x, 2.0.x). * `compat-mrpt-1.3`: Stall branch, archived for backwards compatibility with MRPT versions 1.3.x or older. diff --git a/mrpt_graphslam_2d/.gitignore b/mrpt_graphslam_2d/.gitignore index 393c7d4..ab0b8cc 100644 --- a/mrpt_graphslam_2d/.gitignore +++ b/mrpt_graphslam_2d/.gitignore @@ -1 +1,3 @@ -rosbags/records_with_gt.bag +rosbags/* +rviz/*bag* +rviz/*gazebo* diff --git a/mrpt_graphslam_2d/README.md b/mrpt_graphslam_2d/README.md index 15a5eb5..07ee0fa 100644 --- a/mrpt_graphslam_2d/README.md +++ b/mrpt_graphslam_2d/README.md @@ -7,7 +7,7 @@ mrpt\_graphslam\_2d is heavily based on the native MRPT *graphslam-engine* application. Furthermore the command-line arguments offered by the latter can be provided by the user as parameters in the ROS Parameter Server. See the provided roslaunch files for examples of this. Also see the API of -mrpt\_graphslam\_2d available [here](// TODO - add it.) +mrpt\_graphslam\_2d available [here](http://docs.ros.org/lunar/api/mrpt_graphslam_2d/html/index.html) ## Additional information: @@ -19,7 +19,7 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.) ### Real-Time experiment - short loop -- A sample rosbag is included in the rosbags/demo_short_loop directory. To run +- A sample rosbag is included in the `rosbags/demo_short_loop` directory. To run this just launch the sr_graphslam_demo.launch file: `roslaunch mrpt_graphslam_2d sr_graphslam_demo.launch start_rviz:=True` @@ -36,14 +36,15 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.) ceiling. The cameras using the ar_sys ROS package are tracking Aruco markers that act as the workspace origin (static marker) and as the robot executing SLAM (moving marker) respectively. - To run this demo, just download the whole folder place it directly under - the rosbags directory of the mrpt_graphslam_2d package and - run the sr_graphslam_demo_gt.launch file. + To run this demo, simply download the entire folder, place it directly under + the `rosbags` directory of the mrpt_graphslam_2d package and run the + sr_graphslam_demo_gt.launch file. `roslaunch mrpt_graphslam_2d sr_graphslam_demo_gt.launch` - One can also tinker with the aforementioned launchfile to enable/disable the - different visualization features. + different visualization features, use another decider/optimizer class, or + modify the .ini file to change the algorithm behavior. - Robot movement starts after ~60''. Due to different timestamps in the laserscans, odometry topics the algorithm feedback is lagging a bit compared @@ -61,14 +62,53 @@ mrpt\_graphslam\_2d available [here](// TODO - add it.) ## Multi-robot algorithm +Multi-robot support is provided via the +[csl_mr_slam](http://github.com/bergercookie/csl_mr_slam) package suite as well +as the use of multimaster_fkie packages utilized for communication across the +various ROS agents. + +### Multi-robot real-time experiments - rosbag + +A rosbag collected from real-robots can be found in the following link (more +rosbags to be added in the MRPT-2.0 branch): + +- [multi_robot_graphSLAM_short](https://www.dropbox.com/sh/mxnij0jxvubyu2h/AADC8k6p-ZSq2nipGi4CiesFa?dl=0) + +To run a demo using one of the corresponding rosbags download that directory, +place it directly under the `rosbags` directory of the mrpt_graphslam_2d +package and run the `run_mr_graphslam_demo.sh` script of the `csl_robots_gazebo +package`. The latter script (as is standard with the nodes of the +`csl_robots_gazebo` package reads its configuration parameters off the shell +environment at hand and adjusts its behavior accordingly. As an examaple users +can use the following command for running a multi-robot demo. + +```sh +# see the script for configuration variables +rosrun csl_robots_gazebo run_mr_graphslam_demo.sh +``` + +**Warning** + +- For this to work, you need to have the csl_mr_slam package suite (as well as the +packages that the latter depends on) in your catkin workspace. + +- When executing multi-robot graphSLAM using either Gazebo (via the + `csl_robots_gazebo` package or using measurements from the prerecorded + rosbags, you have to, a priori, generate the necessary rviz files from the + template files found in `$(rospack find mrpt_graphslam_2d)/rviz/templates`. + To do that you have to run the `$(rospack find + mrpt_graphslam_2d)/nodes/rename_rviz_topics.py` script which changes the + necessary topic names based on the running computer's hostname. See + documentation of the latter script for more on its usage. + + ### Multi-robot simulations in Gazebo - [csl_mr_slam](http://github.com/bergercookie/csl_mr_slam) Multi-robot simulations are supported in the Gazebo Simulator via the [csl_hw_setup](https://github.com/bergercookie/csl_mr_slam/tree/master/csl_hw_setup) -ROS package. An example of running such a simulation is given -below. +ROS package. An example of running such a simulation is given below. -![](https://media.giphy.com/media/l0Iy3H3H4eJQFxqlW/giphy.gif) +![](https://media.giphy.com/media/l0Iydx6Dq3T7GSJVK/giphy.gif) A complete example of executing multi-robot graphSLAM in the Gazebo simulation environment is presented in the following video: diff --git a/mrpt_graphslam_2d/config/ros_laser_odometry.ini b/mrpt_graphslam_2d/config/ros_laser_odometry.ini index 917508a..98327cc 100644 --- a/mrpt_graphslam_2d/config/ros_laser_odometry.ini +++ b/mrpt_graphslam_2d/config/ros_laser_odometry.ini @@ -27,9 +27,6 @@ save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -;ground_truth_file_format = RGBD_TUM // variable for determining how to parse the groundtruth file. -ground_truth_file_format = NavSimul - ; min node difference for an edge to be considered as a loop closure ; Used in both the visualization procedure for updating the Loop closures ; counter as well as the optimization procedure since it affects how often we @@ -40,7 +37,7 @@ class_verbosity = 1 ######################################################## [NodeRegistrationDeciderParameters] -registration_max_distance = 0.2 // meters +registration_max_distance = 0.1 // meters registration_max_angle = 20 // degrees ; number of ICP goodness values to consider for estimating the current ICP threshold @@ -53,8 +50,7 @@ class_verbosity = 1 [EdgeRegistrationDeciderParameters] ; Works on the 2D observation-only dataset -ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph - needed by CICPCriteriaERD -use_scan_matching = false +ICP_goodness_thresh = 0.60 // threshold for accepting the ICP constraint in the graph - needed by CICPCriteriaERD ; Either check ICP using distance OR node count from current node ICP_max_distance = 2 // maximum distance for checking other nodes for ICP constraints @@ -75,6 +71,19 @@ scale_hessian = 0.2 tau = 1e-3 class_verbosity = 1 +[MappingParameters] + +resolution=0.03 +mapAltitude = 0 +useMapAltitude = 0 +maxDistanceInsertion = 5 +maxOccupancyUpdateCertainty = 0.80 +considerInvalidRangesAsFreeSpace = 1 +decimation = 1 +horizontalTolerance = 0.00088 +wideningBeamsWithDistance = 1 + + ######################################################## # seems that it doesn't read hex, using cfg_file.read_int # hardcode the integer values instead @@ -126,3 +135,16 @@ ICP_algorithm = icpLevenbergMarquardt # decimation to apply to the point cloud being registered against the map # Reduce to "1" to obtain the best accuracy corresponding_points_decimation = 10 + +[MappingParameters] + +mapAltitude = 0 +useMapAltitude = 0 +maxDistanceInsertion = 5 +maxOccupancyUpdateCertainty = 0.65 +considerInvalidRangesAsFreeSpace = 0 +decimation = 1 +horizontalTolerance = 0.00088 +wideningBeamsWithDistance = 1 + + diff --git a/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini index 220db9a..ca71016 100644 --- a/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini +++ b/mrpt_graphslam_2d/config/ros_laser_odometry_LC_MR_real.ini @@ -27,7 +27,6 @@ save_3DScene = true save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. ; Set the verbosity of the output messages. Only messages over the specified ; will be printed to the console. @@ -36,7 +35,7 @@ ground_truth_file_format = NavSimul // variable for determining how to parse the ; LVL_INFO => 1, ; LVL_WARN => 2, ; LVL_ERROR => 3 -class_verbosity = 0 +class_verbosity = 1 ; min node difference for an edge to be considered as a loop closure @@ -68,7 +67,7 @@ consec_icp_constraint_factor = 0.91 lc_icp_constraint_factor = 0.80 -class_verbosity = 0 +class_verbosity = 1 // Graph Partitioning Parameters forceBisectionOnly = false diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini index 50d3005..196c654 100644 --- a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans.ini @@ -27,7 +27,6 @@ save_3DScene = true save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. ; Set the verbosity of the output messages. Only messages over the specified ; will be printed to the console. @@ -80,6 +79,19 @@ tau = 1e-3 class_verbosity = 1 +[MappingParameters] + +resolution=0.03 +mapAltitude = 0 +useMapAltitude = 0 +maxDistanceInsertion = 5 +maxOccupancyUpdateCertainty = 0.80 +considerInvalidRangesAsFreeSpace = 1 +decimation = 1 +horizontalTolerance = 0.00088 +wideningBeamsWithDistance = 1 + + ######################################################## # seems that it doesn't read hex, using cfg_file.read_int # hardcode the integer values instead diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini index a65f707..b76d49e 100644 --- a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC.ini @@ -27,7 +27,6 @@ save_3DScene = true save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. ; Set the verbosity of the output messages. Only messages over the specified ; will be printed to the console. diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini index 2bfae67..3992ba8 100644 --- a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_real.ini @@ -19,6 +19,7 @@ ############################################################### +######################################################## [GeneralConfiguration] output_dir_fname = graphslam_results @@ -27,7 +28,6 @@ save_3DScene = true save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. ; Set the verbosity of the output messages. Only messages over the specified ; will be printed to the console. @@ -48,7 +48,7 @@ LC_min_nodeid_diff= 10 ####################################################### [NodeRegistrationDeciderParameters] -registration_max_distance = 0.2 +registration_max_distance = 0.1 ;registration_max_distance = 1 // meters registration_max_angle = 15 // degrees class_verbosity = 1 @@ -58,15 +58,14 @@ class_verbosity = 1 use_scan_matching = true // strongly suggested that this is set to TRUE ;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph -prev_nodes_for_ICP = 5 -LC_eigenvalues_ratio_thresh = 2.5 -LC_min_remote_nodes = 10 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures +prev_nodes_for_ICP = 10 +LC_eigenvalues_ratio_thresh = 2 +LC_min_remote_nodes = 2 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures LC_check_curr_partition_only = true consec_icp_constraint_factor = 0.96 lc_icp_constraint_factor = 0.95 - class_verbosity = 1 // Graph Partitioning Parameters @@ -86,10 +85,11 @@ useMapMatching = true // Loop Closing Parameters +######################################################## [OptimizerParameters] -optimization_on_second_thread = false -optimization_distance = 0.4; +/ optimization_on_second_thread = false +optimization_distance = 3.0; ;optimization_distance = -1 // optimize whole graph every time. // Levenberg-Marquardt parameters @@ -101,6 +101,34 @@ tau = 1e-3 class_verbosity = 1 +[MappingParameters] + +resolution=0.05 +mapAltitude = 0 +useMapAltitude = 0 +maxDistanceInsertion = 5 +maxOccupancyUpdateCertainty = 0.80 +considerInvalidRangesAsFreeSpace = 1 +decimation = 1 +horizontalTolerance = 0.00088 +wideningBeamsWithDistance = 1 + +######################################################## +[AlignmentParameters] + +min_ICP_goodness = 0.60 +maxKLd_for_merge = 0.90 +ransac_minSetSizeRatio = 0.40 + +######################################################## +[MultiRobotParameters] + +nodes_integration_batch_size = 5 +num_last_registered_nodes = 10 +conservative_find_initial_tfs_to_neighbors = false +inter_group_node_count_thresh = 30 + + ######################################################## # seems that it doesn't read hex, using cfg_file.read_int # hardcode the integer values instead @@ -131,6 +159,7 @@ visualize_SLAM_metric = true // extra displayPlots showing the evolution visualize_map_partitions = true enable_curr_pos_viewport = true +######################################################## [ICP] maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: diff --git a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_simul.ini similarity index 81% rename from mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini rename to mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_simul.ini index 332393b..9c214bd 100644 --- a/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR.ini +++ b/mrpt_graphslam_2d/config/ros_odometry_2DRangeScans_LC_MR_simul.ini @@ -13,12 +13,13 @@ # NodeRegistrationDecider tested : CFixedIntervalsNRD # EdgeRegistrationDecider : CLoopCloserERD_MR # GraphSlamSolver : CLevMarqGSO -# -# File is used for multi-robot simulations in Gazebo. See the csl_robots_gazebo -# ROS package for more on the configuration +# +# File is used for multi-robot real-time usage. See the csl_hw_setup +# ROS package for more on the configuration. ############################################################### +######################################################## [GeneralConfiguration] output_dir_fname = graphslam_results @@ -27,7 +28,6 @@ save_3DScene = true save_3DScene_fname = output_scene.3DScene save_graph = true save_graph_fname = output_graph.graph -ground_truth_file_format = NavSimul // variable for determining how to parse the groundtruth file. ; Set the verbosity of the output messages. Only messages over the specified ; will be printed to the console. @@ -36,7 +36,7 @@ ground_truth_file_format = NavSimul // variable for determining how to parse the ; LVL_INFO => 1, ; LVL_WARN => 2, ; LVL_ERROR => 3 -class_verbosity = 0 +class_verbosity = 1 ; min node difference for an edge to be considered as a loop closure @@ -48,7 +48,7 @@ LC_min_nodeid_diff= 10 ####################################################### [NodeRegistrationDeciderParameters] -registration_max_distance = 0.4 +registration_max_distance = 0.3 ;registration_max_distance = 1 // meters registration_max_angle = 15 // degrees class_verbosity = 1 @@ -58,17 +58,15 @@ class_verbosity = 1 use_scan_matching = true // strongly suggested that this is set to TRUE ;ICP_goodness_thresh = 0.80 // threshold for accepting the ICP constraint in the graph -prev_nodes_for_ICP = 10 -;prev_nodes_for_ICP = 5 +prev_nodes_for_ICP = 5 LC_eigenvalues_ratio_thresh = 2 -LC_min_remote_nodes = 2 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures +LC_min_remote_nodes = 3 // how many out "remote" nodes should exist in a partition for the partition to be examined for potential loop closures LC_check_curr_partition_only = true -consec_icp_constraint_factor = 0.91 -lc_icp_constraint_factor = 0.80 +consec_icp_constraint_factor = 0.96 +lc_icp_constraint_factor = 0.95 - -class_verbosity = 0 +class_verbosity = 1 // Graph Partitioning Parameters forceBisectionOnly = false @@ -87,9 +85,10 @@ useMapMatching = true // Loop Closing Parameters +######################################################## [OptimizerParameters] -optimization_on_second_thread = false +/ optimization_on_second_thread = false optimization_distance = 0.4; ;optimization_distance = -1 // optimize whole graph every time. @@ -102,6 +101,34 @@ tau = 1e-3 class_verbosity = 1 +[MappingParameters] + +resolution=0.05 +mapAltitude = 0 +useMapAltitude = 0 +maxDistanceInsertion = 10 +maxOccupancyUpdateCertainty = 0.80 +considerInvalidRangesAsFreeSpace = 1 +decimation = 1 +horizontalTolerance = 0.00088 +wideningBeamsWithDistance = 1 + +######################################################## +[AlignmentParameters] + +min_ICP_goodness = 0.60 +maxKLd_for_merge = 0.90 +ransac_minSetSizeRatio = 0.40 + +######################################################## +[MultiRobotParameters] + +nodes_integration_batch_size = 5 +num_last_registered_nodes = 10 +conservative_find_initial_tfs_to_neighbors = false +inter_group_node_count_thresh = 30 + + ######################################################## # seems that it doesn't read hex, using cfg_file.read_int # hardcode the integer values instead @@ -132,6 +159,7 @@ visualize_SLAM_metric = true // extra displayPlots showing the evolution visualize_map_partitions = true enable_curr_pos_viewport = true +######################################################## [ICP] maxIterations = 100 // The maximum number of iterations to execute if convergence is not achieved before minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h index 3166d90..02834db 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR.h @@ -68,7 +68,7 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS typedef typename mrpt::graphslam::detail::TNodeProps node_props_t; typedef mrpt::graphslam::TUncertaintyPath path_t; typedef std::vector paths_t; - typedef CEdgeRegistrationDecider_MR edge_reg_mr_t; + typedef mrpt::graphslam::deciders::CEdgeRegistrationDecider_MR edge_reg_mr_t; CGraphSlamEngine_MR( ros::NodeHandle* nh, @@ -286,11 +286,11 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS private: - /**\brief Traverse all neighbors for which I know the intra-graph + /**\brief Traverse all neighbors for which I know the inter-graph * transformation, run addNodeBatchFromNeighbor. */ bool addNodeBatchesFromAllNeighbors(); - /**\brief neighbors for which I know the intra-graph + /**\brief neighbors for which I know the inter-graph * transformation, add new batches of fetches nodeIDs and scans * * Try to integrate the newly received nodeIDs and laser Scans in own graph @@ -365,11 +365,8 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS mrpt_msgs::GetCMGraph::Response& res); void readParams(); - /**\brief Read the parameters related to the map-matching operation - */ - void readGridMapAlignmentParams(); void readROSParameters(); - void printParams(); + void printParams() const; mrpt::poses::CPose3D getLSPoseForGridMapVisualization( const mrpt::utils::TNodeID nodeID) const; void setObjectPropsFromNodeID( @@ -462,15 +459,6 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS */ size_t m_graph_nodes_last_size; - /**\brief Max number of last registered NodeIDs + corresponding positions to publish. - * - * This is necessary for the other GraphSLAM agents so that they can use this - * information to localize the current agent in their own map and later - * make a querry for the Condensed Measurements Graph. - */ - int m_num_last_regd_nodes; - - /**\brief CConnectionManager instance */ mrpt::graphslam::detail::CConnectionManager m_conn_manager; @@ -501,17 +489,6 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS */ bool m_registered_multiple_nodes; - /**\brief Lowest number of nodes that should exist in a group of nodes before - * evaluating it. - * - * \note This should be set >= 3 - * \sa m_intra_group_node_count_thresh_minadv - */ - int m_intra_group_node_count_thresh; - /**\brief Minimum advised limit of m_intra_group_node_count_thresh - * \sa m_intra_group_node_count_thresh - */ - int m_intra_group_node_count_thresh_minadv; /**\brief AsyncSpinner that is used to query the CM-Graph service in case a * new request arrives */ @@ -526,24 +503,62 @@ class CGraphSlamEngine_MR : public CGraphSlamEngine_ROS */ bool m_pause_exec_on_mr_registration; - /**\brief After an intra-graph transformation is found between own graph and - * a neighbor's map, newly fetched neighbor's nodes are added in batches. - */ - int m_nodes_integration_batch_size; - /**\brief Parameters used during the alignment operation */ mrpt::slam::CGridMapAligner::TConfigParams m_alignment_options; - /**\brief Be conservative when it comes to deciding the initial - * transformation of own graph with regards to graphs of the neighboring - * agents. If true engine won't use map merging but instead will be waiting - * for rendez-vous with other agents to determine the tf. - * - * \warning Rendez-vous behavior is not yet implemented. - */ - bool m_conservative_find_initial_tfs_to_neighbors; - mrpt::utils::TColorManager neighbor_colors_manager; + mrpt::utils::TColorManager m_neighbor_colors_manager; + + std::string m_sec_alignment_params; + std::string m_sec_mr_slam_params; + + struct TOptions : mrpt::utils::CLoadableOptions { + + typedef self_t engine_mr_t; + + TOptions(const engine_mr_t& engine_in); + ~TOptions(); + void loadFromConfigFile( + const mrpt::utils::CConfigFileBase& source, + const std::string& section); + void dumpToTextStream(mrpt::utils::CStream& out) const; + + + /**\brief Be conservative when it comes to deciding the initial + * transformation of own graph with regards to graphs of the neighboring + * agents. If true engine won't use map merging but instead will be waiting + * for rendez-vous with other agents to determine the tf. + * + * \warning Rendez-vous behavior is not yet implemented. + */ + bool conservative_find_initial_tfs_to_neighbors; + /**\brief After an inter-graph transformation is found between own graph and + * a neighbor's map, newly fetched neighbor's nodes are added in batches. + */ + int nodes_integration_batch_size; + /**\brief Max number of last registered NodeIDs + corresponding positions to publish. + * + * This is necessary for the other GraphSLAM agents so that they can use this + * information to localize the current agent in their own map and later + * make a querry for the Condensed Measurements Graph. + */ + int num_last_regd_nodes; + /**\brief Lowest number of nodes that should exist in a group of nodes before + * evaluating it. These nodes are fetched by the other running graphSLAM agents + * + * \note This should be set >= 3 + * \sa inter_group_node_count_thresh_minadv + */ + int inter_group_node_count_thresh; + /**\brief Minimum advised limit of inter_group_node_count_thresh + * \sa inter_group_node_count_thresh + */ + int inter_group_node_count_thresh_minadv; + + /** Instance of engine which uses this struct */ + const engine_mr_t& engine; + + } m_opts; }; diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h index 073a2d0..8331bdb 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_MR_impl.h @@ -35,9 +35,11 @@ CGraphSlamEngine_MR::CGraphSlamEngine_MR( m_nh(nh), m_graph_nodes_last_size(0), m_registered_multiple_nodes(false), - m_intra_group_node_count_thresh_minadv(25), cm_graph_async_spinner(/* threads_num: */ 1, &this->custom_service_queue), - m_pause_exec_on_mr_registration(false) + m_pause_exec_on_mr_registration(false), + m_sec_alignment_params("AlignmentParameters"), + m_sec_mr_slam_params("MultiRobotParameters"), + m_opts(*this) { this->initClass(); } @@ -65,9 +67,9 @@ bool CGraphSlamEngine_MR::addNodeBatchesFromAllNeighbors() { bool ret_val = false; for (const auto& neighbor : m_neighbors) { if (!isEssentiallyZero( - neighbor->tf_self_to_neighbor_first_integrated_pose) && // intra-graph TF found + neighbor->tf_self_to_neighbor_first_integrated_pose) && // inter-graph TF found neighbor->hasNewData() && - neighbor->hasNewNodesBatch(m_nodes_integration_batch_size)) { + neighbor->hasNewNodesBatch(m_opts.nodes_integration_batch_size)) { bool loc_ret_val = this->addNodeBatchFromNeighbor(neighbor); if (loc_ret_val) { ret_val = true; } } @@ -207,7 +209,7 @@ bool CGraphSlamEngine_MR::findTFsWithAllNeighbors() { using namespace mrpt::utils; using namespace mrpt::math; - if (this->m_graph.nodeCount() < m_intra_group_node_count_thresh) { + if (this->m_graph.nodeCount() < m_opts.inter_group_node_count_thresh) { return false; } @@ -225,7 +227,7 @@ bool CGraphSlamEngine_MR::findTFsWithAllNeighbors() { ++neighbors_it) { // run matching proc with neighbor only if I haven't found an initial - // intra-graph TF + // inter-graph TF if (!m_neighbor_to_found_initial_tf.at(*neighbors_it)) { bool loc_ret_val = findTFWithNeighbor(*neighbors_it); @@ -263,7 +265,7 @@ findTFWithNeighbor(TNeighborAgentProps* neighbor) { vector_uint neighbor_nodes; std::map nodes_params; neighbor->getCachedNodes(&neighbor_nodes, &nodes_params, /*only unused=*/ false); - if (neighbor_nodes.size() < m_intra_group_node_count_thresh || + if (neighbor_nodes.size() < m_opts.inter_group_node_count_thresh || !neighbor->hasNewData()) { return false; } @@ -283,8 +285,11 @@ findTFWithNeighbor(TNeighborAgentProps* neighbor) { this->logFmt(LVL_INFO, "Trying to align the maps, initial estimation: %s", init_estim.mean.asString().c_str()); - neighbor_gridmap->saveMetricMapRepresentationToFile(this->getLoggerName() + "_other"); - this->m_gridmap_cached->saveMetricMapRepresentationToFile(this->getLoggerName() + "_self"); + + // Save them for debugging reasons + //neighbor_gridmap->saveMetricMapRepresentationToFile(this->getLoggerName() + "_other"); + //this->m_gridmap_cached->saveMetricMapRepresentationToFile(this->getLoggerName() + "_self"); + const CPosePDFPtr pdf_tmp = gridmap_aligner.AlignPDF( this->m_gridmap_cached.pointer(), neighbor_gridmap.pointer(), init_estim, @@ -442,11 +447,11 @@ bool CGraphSlamEngine_MR::_execGraphSlamStep( // find matches between own nodes and those of the neighbors bool did_register_from_map_merge; - if (!m_conservative_find_initial_tfs_to_neighbors) { + if (!m_opts.conservative_find_initial_tfs_to_neighbors) { did_register_from_map_merge = this->findTFsWithAllNeighbors(); } - else { // intra-graph TF is available - add new nodes - THROW_EXCEPTION("Conservative intra-graph TF computation is not yet implemented."); + else { // inter-graph TF is available - add new nodes + THROW_EXCEPTION("Conservative inter-graph TF computation is not yet implemented."); } bool did_register_from_batches = this->addNodeBatchesFromAllNeighbors(); @@ -466,6 +471,7 @@ template void CGraphSlamEngine_MR::initClass() { using namespace mrpt::graphslam; using namespace mrpt::utils; + using namespace std; // initialization of topic namespace names // TODO - put these into seperate method @@ -572,7 +578,7 @@ void CGraphSlamEngine_MR::initClass() { } this->readParams(); - this->m_optimized_map_color = neighbor_colors_manager.getNextTColor(); + this->m_optimized_map_color = m_neighbor_colors_manager.getNextTColor(); // start the spinner for asynchronously servicing cm_graph requests cm_graph_async_spinner.start(); @@ -674,7 +680,7 @@ void CGraphSlamEngine_MR::usePublishersBroadcasters() { << gsa.name.data); m_neighbors.push_back(new TNeighborAgentProps(*this, gsa)); TNeighborAgentProps* latest_neighbor = m_neighbors.back(); - latest_neighbor->setTColor(neighbor_colors_manager.getNextTColor()); + latest_neighbor->setTColor(m_neighbor_colors_manager.getNextTColor()); m_neighbor_to_found_initial_tf.insert(make_pair( latest_neighbor, false)); latest_neighbor->setupComm(); @@ -706,10 +712,10 @@ bool CGraphSlamEngine_MR::pubUpdatedNodesList() { // fill the NodeIDWithPose_vec msg NodeIDWithPose_vec ros_nodes; - // send up to m_num_last_regd_nodes Nodes - start from end. + // send up to num_last_regd_nodes Nodes - start from end. for (typename GRAPH_T::global_poses_t::const_reverse_iterator cit = this->m_graph.nodes.rbegin(); - (poses_counter <= m_num_last_regd_nodes && + (poses_counter <= m_opts.num_last_regd_nodes && cit != this->m_graph.nodes.rend()); ++cit) { @@ -832,34 +838,20 @@ bool CGraphSlamEngine_MR::isOwnNodeID( template void CGraphSlamEngine_MR::readParams() { - this->readROSParameters(); - this->readGridMapAlignmentParams(); -} - -template -void CGraphSlamEngine_MR::readGridMapAlignmentParams() { + using namespace mrpt::utils; using namespace mrpt::slam; - std::string alignment_opts_ns = "alignment_opts"; + this->readROSParameters(); - m_nh->param(alignment_opts_ns + "/" + "min_ICP_goodness", - m_alignment_options.min_ICP_goodness, 0.60); - m_nh->param(alignment_opts_ns + "/" + "maxKLd_for_merge", - m_alignment_options.maxKLd_for_merge, 0.90); - m_nh->param(alignment_opts_ns + "/" + "ransac_minSetSizeRatio", - m_alignment_options.ransac_minSetSizeRatio, 0.40); - m_nh->param(alignment_opts_ns + "/" + "nodes_integration_batch_size", - m_nodes_integration_batch_size, 4); + // GridMap Alignment + m_alignment_options.loadFromConfigFileName(this->m_config_fname, m_sec_alignment_params); - // GridMap Alignment options to be used in merging. + // Thu Jun 8 17:02:17 EEST 2017, Nikos Koukis + // other option ends up in segfault. m_alignment_options.methodSelection = CGridMapAligner::amModifiedRANSAC; - ////options.methodSelection = CGridMapAligner::amRobustMatch; // ASSERTION ERROR - //m_alignment_options.loadFromConfigFileName( - //"/home/bergercookie/mrpt/share/mrpt/config_files/grid-matching/gridmatch_example.ini", - //"grid-match"); - -} + m_opts.loadFromConfigFileName(this->m_config_fname, m_sec_mr_slam_params); +} // end of readParams template void CGraphSlamEngine_MR::readROSParameters() { @@ -867,30 +859,13 @@ void CGraphSlamEngine_MR::readROSParameters() { // parameters parent_t::readROSParameters(); - m_nh->param(m_mr_ns + "/" + "conservative_find_initial_tfs_to_neighbors", - m_conservative_find_initial_tfs_to_neighbors, false); - ASSERTMSG_(!m_conservative_find_initial_tfs_to_neighbors, - "Conservative behavior is not yet implemented."); - - m_nh->param(m_mr_ns + "/" + "num_last_registered_nodes", - m_num_last_regd_nodes, 10); - - m_nh->param(m_mr_ns + "/" + "intra_group_node_count_thresh", - m_intra_group_node_count_thresh, m_intra_group_node_count_thresh_minadv); - // warn user if they choose smaller threshold. - if (m_intra_group_node_count_thresh < m_intra_group_node_count_thresh_minadv) { - MRPT_LOG_ERROR_STREAM("intra_group_node_count_thresh [" - << m_intra_group_node_count_thresh - << "is set lower than the advised minimum [" - << m_intra_group_node_count_thresh_minadv - << "]"); - } } template -void CGraphSlamEngine_MR::printParams() { +void CGraphSlamEngine_MR::printParams() const { parent_t::printParams(); m_alignment_options.dumpToConsole(); + m_opts.dumpToConsole(); } @@ -1078,9 +1053,17 @@ CGraphSlamEngine_MR::TNeighborAgentProps::TNeighborAgentProps( "In constructor of TNeighborAgentProps for topic namespace: %s", agent.topic_namespace.data.c_str()); - // initialize the occupancy map + // initialize the occupancy map based on the engine's gridmap properties gridmap_cached = mrpt::maps::COccupancyGridMap2D::Create(); -} + COccupancyGridMap2DPtr eng_gridmap = engine.m_gridmap_cached; + gridmap_cached->setSize( + eng_gridmap->getXMin(), + eng_gridmap->getXMax(), + eng_gridmap->getYMin(), + eng_gridmap->getYMax(), + eng_gridmap->getResolution()); + +} // TNeighborAgentProps::TNeighborAgentProps template CGraphSlamEngine_MR::TNeighborAgentProps::~TNeighborAgentProps() { } @@ -1394,6 +1377,7 @@ computeGridMap() const { using namespace mrpt::utils; using namespace mrpt; using namespace mrpt::math; + using namespace std; gridmap_cached->clear(); @@ -1412,7 +1396,7 @@ computeGridMap() const { } MRPT_END; -} +} // end of TNeighborAgentProps::computeGridMap template const mrpt::maps::COccupancyGridMap2DPtr& @@ -1425,7 +1409,7 @@ getGridMap() const { return gridmap_cached; MRPT_END; -} +} // end of TNeighborAgentProps::getGridMap template void CGraphSlamEngine_MR::TNeighborAgentProps:: @@ -1439,12 +1423,70 @@ getGridMap(mrpt::maps::COccupancyGridMap2DPtr& map) const { map->copyMapContentFrom(*gridmap_cached); MRPT_END; -} +} // end of TNeighborAgentProps::getGridMap + template bool CGraphSlamEngine_MR::TNeighborAgentProps::hasNewData() const { return has_new_nodes && has_new_scans; -} +} // end of hasNewData + +template +CGraphSlamEngine_MR::TOptions::TOptions(const engine_mr_t& engine_in): + nodes_integration_batch_size(4), + num_last_regd_nodes(10), + conservative_find_initial_tfs_to_neighbors(false), + inter_group_node_count_thresh_minadv(25), + inter_group_node_count_thresh(40), + engine(engine_in) +{ +} // end of TOptions::TOptions + +template +CGraphSlamEngine_MR::TOptions::~TOptions() { +} // end of TOptions::~TOptions + +template +void CGraphSlamEngine_MR::TOptions::loadFromConfigFile( + const mrpt::utils::CConfigFileBase& source, + const std::string& section) { + + MRPT_LOAD_CONFIG_VAR(nodes_integration_batch_size, int, source, section); + MRPT_LOAD_CONFIG_VAR(num_last_regd_nodes, int, source, section); + + MRPT_LOAD_CONFIG_VAR(conservative_find_initial_tfs_to_neighbors, bool, source, section); + ASSERTMSG_(!conservative_find_initial_tfs_to_neighbors, + "Conservative behavior is not yet implemented."); + + + MRPT_LOAD_CONFIG_VAR(inter_group_node_count_thresh, int, source, section); + // warn user if they choose smaller threshold. + if (inter_group_node_count_thresh < inter_group_node_count_thresh_minadv) { + engine.logFmt(mrpt::utils::LVL_ERROR, + "inter_group_node_count_thresh [%d]" + "is set lower than the advised minimum [%d]", + inter_group_node_count_thresh, + inter_group_node_count_thresh_minadv); + + mrpt::system::sleep(2000); + } + + +} // end of TOptions::loadFromConfigFile + +template +void CGraphSlamEngine_MR::TOptions::dumpToTextStream( + mrpt::utils::CStream& out) const { + + LOADABLEOPTS_DUMP_VAR(nodes_integration_batch_size, int); + LOADABLEOPTS_DUMP_VAR(num_last_regd_nodes, int); + LOADABLEOPTS_DUMP_VAR(conservative_find_initial_tfs_to_neighbors, bool); + LOADABLEOPTS_DUMP_VAR(inter_group_node_count_thresh, int) + LOADABLEOPTS_DUMP_VAR(inter_group_node_count_thresh_minadv, int) + +} // end of TOptions::dumpToTextStream + + } } // end of namespaces diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h index 4251e2e..0ccee31 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamEngine_ROS.h @@ -53,7 +53,7 @@ class CGraphSlamEngine_ROS : public CGraphSlamEngine protected: /**\brief Read the problem configuration parameters * - * \sa readROSParameters + * \sa readROSParameters, CGraphSlamEngine::loadParams */ void readParams(); /**\brief Provide feedback about the SLAM operation @@ -65,7 +65,6 @@ class CGraphSlamEngine_ROS : public CGraphSlamEngine /**\brief Read configuration parameters from the ROS parameter server. * * \note Method is automatically called on object construction. - * * \sa readParams, initClass */ void readROSParameters(); diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h index 243abc8..b69a55b 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS.h @@ -213,14 +213,15 @@ class CGraphSlamHandler_ROS : std::string m_optimizer; /**\}*/ - /**\brief Minimum logging level - * + /**\brief Minimum logging level for the current class instance. + * + * This doesn't affect the logging level of CGraphSlamEngine or any of the + * deciders/optimizers. * \note Value is fetched from the ROS Parameter Server (not from the * external .ini file. */ mrpt::utils::VerbosityLevel m_min_logging_level; - bool m_has_read_config; /**\name Received measurements - boolean flags * * \brief Flags that indicate if any new measurements have arrived in the diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h index 7a24924..be44bcd 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/CGraphSlamHandler_ROS_impl.h @@ -41,7 +41,6 @@ CGraphSlamHandler_ROS::CGraphSlamHandler_ROS( m_queue_size = 1; // variables initialization/assignment - m_has_read_config = false; m_pub_seq = 0; m_stats_pub_seq = 0; this->resetReceivedFlags(); @@ -69,7 +68,6 @@ void CGraphSlamHandler_ROS::readParams() { ASSERT_(!this->m_ini_fname.empty()); parent_t::readConfigFname(this->m_ini_fname); - m_has_read_config = true; } template @@ -78,55 +76,52 @@ void CGraphSlamHandler_ROS::readROSParameters() { // misc { - std::string ns = "misc/"; - - // enable/disable visuals - bool m_disable_MRPT_visuals; - m_nh->param(ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false); - this->m_enable_visuals = !m_disable_MRPT_visuals; - - // verbosity level - int lvl; - m_nh->param(ns + "verbosity", - lvl, - static_cast(LVL_INFO)); - m_min_logging_level = static_cast(lvl); - this->m_logger->setMinLoggingLevel(m_min_logging_level); + std::string ns = "misc/"; + + // enable/disable visuals + bool m_disable_MRPT_visuals; + m_nh->param(ns + "disable_MRPT_visuals", m_disable_MRPT_visuals, false); + this->m_enable_visuals = !m_disable_MRPT_visuals; + + // verbosity level + int lvl; + m_nh->param(ns + "verbosity", + lvl, + static_cast(LVL_INFO)); + m_min_logging_level = static_cast(lvl); + this->m_logger->setMinLoggingLevel(m_min_logging_level); } // deciders, optimizer { - std::string ns = "deciders_optimizers/"; - m_nh->param(ns + "NRD", m_node_reg, "CFixedIntervalsNRD"); - m_nh->param(ns + "ERD", m_edge_reg, "CICPCriteriaERD"); - m_nh->param(ns + "GSO", m_optimizer, "CLevMarqGSO"); + std::string ns = "deciders_optimizers/"; + m_nh->param(ns + "NRD", m_node_reg, "CFixedIntervalsNRD"); + m_nh->param(ns + "ERD", m_edge_reg, "CICPCriteriaERD"); + m_nh->param(ns + "GSO", m_optimizer, "CLevMarqGSO"); } // filenames { - std::string ns = "files/"; - - // configuration file - mandatory - std::string config_param_path = ns + "config"; - bool found_config = m_nh->getParam(ns + "config", this->m_ini_fname); - ASSERTMSG_(found_config, - mrpt::format( - "Configuration file was not set. Set %s and try again.\nExiting...", - config_param_path.c_str())); - - // ground-truth file - m_nh->getParam(ns + "ground_truth", this->m_gt_fname); + std::string ns = "files/"; + + // configuration file - mandatory + std::string config_param_path = ns + "config"; + bool found_config = m_nh->getParam(ns + "config", this->m_ini_fname); + ASSERTMSG_(found_config, + mrpt::format( + "Configuration file was not set. Set %s and try again.\nExiting...", + config_param_path.c_str())); + + // ground-truth file + m_nh->getParam(ns + "ground_truth", this->m_gt_fname); } // TF Frame IDs // names of the frames of the corresponding robot parts { - std::string ns = "frame_IDs/"; + std::string ns = "frame_IDs/"; - m_nh->param(ns + "anchor_frame" , m_anchor_frame_id, "map"); - m_nh->param(ns + "base_link_frame" , m_base_link_frame_id, "base_link"); - m_nh->param(ns + "odometry_frame" , m_odom_frame_id, "odom"); - - // take action based on the above frames - // + m_nh->param(ns + "anchor_frame" , m_anchor_frame_id, "map"); + m_nh->param(ns + "base_link_frame" , m_base_link_frame_id, "base_link"); + m_nh->param(ns + "odometry_frame" , m_odom_frame_id, "odom"); } @@ -137,11 +132,11 @@ void CGraphSlamHandler_ROS::readROSParameters() { this->verifyUserInput(); this->m_logger->logFmt(LVL_DEBUG, - "Successfully read parameters from ROS Parameter Server"); + "Successfully read parameters from ROS Parameter Server"); // Visuals initialization if (this->m_enable_visuals) { - this->initVisualization(); + this->initVisualization(); } } // end of readROSParameters @@ -193,6 +188,7 @@ void CGraphSlamHandler_ROS::initEngine_MR() { template void CGraphSlamHandler_ROS::getROSParameters(std::string* str_out) { using namespace mrpt::utils; + using namespace std; ASSERT_(str_out); @@ -256,8 +252,11 @@ std::string CGraphSlamHandler_ROS::getParamsAsString() { template void CGraphSlamHandler_ROS::printParams() { + using namespace std; parent_t::printParams(); cout << this->getParamsAsString() << endl; + + } template @@ -321,7 +320,7 @@ void CGraphSlamHandler_ROS::verifyUserInput() { this->m_gt_fname.c_str())); } -} +} // end of verifyUserInput template void CGraphSlamHandler_ROS::setupComm() { @@ -337,7 +336,7 @@ void CGraphSlamHandler_ROS::setupComm() { // fetch the static geometrical transformations //this->readStaticTFs(); -} +} // end of setupComm template void CGraphSlamHandler_ROS::setupSubs() { @@ -367,7 +366,7 @@ void CGraphSlamHandler_ROS::setupSubs() { // 3D point clouds // TODO -} +} // end of setupSubs template void CGraphSlamHandler_ROS::setupPubs() { @@ -416,20 +415,21 @@ void CGraphSlamHandler_ROS::setupPubs() { m_queue_size, /*latch=*/true); -} +} // end of setupPubs template void CGraphSlamHandler_ROS::setupSrvs() { this->m_logger->logFmt(LVL_INFO, "Setting up the services..."); - // SLAM statistics - // Error statistics + // TODO Error statistics -} +} // end of setupSrvs template bool CGraphSlamHandler_ROS::usePublishersBroadcasters() { using namespace mrpt::utils; + using namespace std; + MRPT_START; bool ret_val = true; diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h index a7d3f2f..f520074 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/ERD/CLoopCloserERD_MR_impl.h @@ -59,7 +59,7 @@ void CLoopCloserERD_MR::fetchNodeIDsForScanMatching( size_t fetched_nodeIDs = 0; for (int nodeID_i = static_cast(curr_nodeID)-1; - ((fetched_nodeIDs <= this->m_laser_params.prev_nodes_for_ICP) && + ((fetched_nodeIDs <= this->m_prev_nodes_for_ICP) && (nodeID_i >= 0)); --nodeID_i) { bool is_own = this->m_engine->isOwnNodeID(nodeID_i); diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h index 9b31694..41959d4 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS.h @@ -48,6 +48,7 @@ struct TUserOptionsChecker_ROS: TUserOptionsChecker_ROS(); ~TUserOptionsChecker_ROS(); + /**\brief Create deciders, optimizers specific to the ROS case */ void createDeciderOptimizerMappings(); void populateDeciderOptimizerProperties(); diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h index 52ff3db..5a8868f 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/TUserOptionsChecker_ROS_impl.h @@ -20,7 +20,8 @@ TUserOptionsChecker_ROS::~TUserOptionsChecker_ROS() { } template -void TUserOptionsChecker_ROS::createDeciderOptimizerMappings() { +void TUserOptionsChecker_ROS:: +createDeciderOptimizerMappings() { using namespace std; using namespace mrpt::graphs; using namespace mrpt::graphslam::apps; @@ -33,14 +34,13 @@ void TUserOptionsChecker_ROS::createDeciderOptimizerMappings() { this->node_regs_map["CFixedIntervalsNRD_MR"] = parent::template createNodeRegistrationDecider>; - // edge registration deciders this->edge_regs_map["CLoopCloserERD_MR"] = parent::template createEdgeRegistrationDecider>; // optimizers -} +} // end of createDeciderOptimizerMappings template void TUserOptionsChecker_ROS::populateDeciderOptimizerProperties() { diff --git a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h index 204b272..85ec30d 100644 --- a/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h +++ b/mrpt_graphslam_2d/include/mrpt_graphslam_2d/interfaces/CRegistrationDeciderOrOptimizer_MR.h @@ -39,8 +39,7 @@ class CRegistrationDeciderOrOptimizer_MR : /**\brief Pointer to the CConnectionManager instance */ mrpt::graphslam::detail::CConnectionManager* m_conn_manager; - /**\brief Constant pointer to the CGraphSlamEngine instance. - * + /**\brief Constant pointer to the CGraphSlamEngine_MR instance. */ const engine_t* m_engine; std::string own_ns; diff --git a/mrpt_graphslam_2d/launch/mr_graphslam.launch b/mrpt_graphslam_2d/launch/mr_graphslam.launch deleted file mode 100644 index 53b3122..0000000 --- a/mrpt_graphslam_2d/launch/mr_graphslam.launch +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/mrpt_graphslam_2d/launch/setup_robot_for_mr_demo.launch b/mrpt_graphslam_2d/launch/setup_robot_for_mr_demo.launch new file mode 100644 index 0000000..f1e884a --- /dev/null +++ b/mrpt_graphslam_2d/launch/setup_robot_for_mr_demo.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch b/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch index bd2c539..0457915 100644 --- a/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch +++ b/mrpt_graphslam_2d/launch/sr_graphslam_demo.launch @@ -3,55 +3,66 @@ - - - - - - - - - + + + + + + + + + + - - + + - - - + + + + + - + + + + + - - + + diff --git a/mrpt_graphslam_2d/launch/sr_graphslam_demo_gt.launch b/mrpt_graphslam_2d/launch/sr_graphslam_demo_gt.launch index 58289ae..128edb6 100644 --- a/mrpt_graphslam_2d/launch/sr_graphslam_demo_gt.launch +++ b/mrpt_graphslam_2d/launch/sr_graphslam_demo_gt.launch @@ -19,29 +19,29 @@ See the README.md file for more information on this - - - + + + - - - - + + + - - - + + + - - - + + + + - - - - + + + + diff --git a/mrpt_graphslam_2d/nodes/rename_rviz_topics.py b/mrpt_graphslam_2d/nodes/rename_rviz_topics.py new file mode 100755 index 0000000..73723bf --- /dev/null +++ b/mrpt_graphslam_2d/nodes/rename_rviz_topics.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +# Tue Jun 13 17:21:40 EEST 2017, Nikos Koukis + +""" +Current script generates rviz files based on the templates files of +rviz/templates and usign the current computer's hostname for determining the +namespaces of the topics. This is a required step for visualizing multi-robot +graphSLAM when running based on rosbags or based on measurements from Gazebo +since each multi-robot agent runs on a separate ROS core to simulate as much as +possible a real-time setup. + +""" + +import os +import logging +import socket + +logging.basicConfig(level=logging.DEBUG) +logger = logging.getLogger("RvizRenamer") + + +def rename_topics_in_rviz_file(templ_file, replace_dict): + """Read from a single template file and write the modified contents to an + actual rviz file one directory prior to the templates directory. + """ + + with open(templ_file, 'r') as templ: + templ_lines = templ.readlines() + head, tail = os.path.split(templ_file) + + # create the actual rviz files in the parent dir of templates + actual_file = os.path.join(os.path.dirname(head), tail) + logger.info("Writing file: %s" % os.path.abspath(actual_file)) + with open(actual_file, 'w') as f: + # rewrite its line of the templ file by making the appropriate + # substitution + f.writelines([l.format(**replace_dict) for l in templ_lines]) + + + +def main(): + """Main.""" + + logger.info("Initializing...") + + # fetch the rviz files that topics renaming is to happen + script_dir = os.path.dirname(os.path.realpath(__file__)) + rviz_dir_path = os.path.join(script_dir, "..", "rviz", "templates") + + rviz_templ_files = [os.path.join(rviz_dir_path, i) + for i in os.listdir(rviz_dir_path)] + + # do the renaming only for files used for "gazebo" (suffix "gazebo") or + # recorded with rosbag (suffix "gazebo") + + rviz_templ_files = filter(lambda name: "bag" in name or "gazebo" in name, + rviz_templ_files) + + curr_hostname = socket.gethostname() + replace_dict = {"HOSTNAME_PLACEHOLDER": curr_hostname} + + logger.info("Rviz files to operate on:\n%s\n\n" % + os.linesep.join([os.path.abspath(f) + for f in rviz_templ_files])) + + # print the strings to be replaced + logger.info("Replacing: %s ==> %s" % replace_dict.items()[0]) + + for templ_file in rviz_templ_files: + rename_topics_in_rviz_file(templ_file, replace_dict) + logger.info("All done.") + + +if __name__ == "__main__": + main() + + + diff --git a/mrpt_graphslam_2d/package.xml b/mrpt_graphslam_2d/package.xml index db0d59d..a67b144 100644 --- a/mrpt_graphslam_2d/package.xml +++ b/mrpt_graphslam_2d/package.xml @@ -38,7 +38,7 @@ mrpt_msgs tf multimaster_msgs_fkie - rviz + tf2_ros diff --git a/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz b/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz index 68c2787..ca0ef72 100644 --- a/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz +++ b/mrpt_graphslam_2d/rviz/graphslam_real_1.rviz @@ -12,12 +12,10 @@ Panels: - /Image1 - /Map1 - /Axes1 - - /Axes2 - /estim_pose1 - - /estim_pose1/Status1 - /odom_path1 Splitter Ratio: 0.5 - Tree Height: 477 + Tree Height: 693 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -118,7 +116,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Class: rviz/Image Enabled: true Image Topic: /nickkoukubuntu/onboard_camera/image_raw @@ -152,7 +150,7 @@ Visualization Manager: Axes Radius: 0.1 Class: rviz/Pose Color: 255; 25; 0 - Enabled: false + Enabled: true Head Length: 0.3 Head Radius: 0.1 Name: estim_pose @@ -161,7 +159,7 @@ Visualization Manager: Shape: Arrow Topic: /nickkoukubuntu/feedback/robot_position Unreliable: false - Value: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -208,16 +206,16 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 3.4002 + Distance: 14.7136 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.397505 - Y: -2.73194 - Z: 1.58828 + X: -0.505443 + Y: 0.239488 + Z: 0.237929 Name: Current View Near Clip Distance: 0.01 Pitch: 0.8654 @@ -228,12 +226,12 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1027 + Height: 1401 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000360fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000410000026c000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002b3000000ee0000001600ffffff000000010000010f00000360fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000360000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001ee000004b9fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c00000365000000f200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003970000014e0000002700ffffff000000010000015b000004b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c000004b9000000dd00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a000000042f00fffffffb0000000800540069006d00650100000000000004500000000000000000000006ab000004b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -242,6 +240,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 + Width: 2560 X: 0 - Y: 31 + Y: 39 diff --git a/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz b/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz index 502b575..d512f9b 100644 --- a/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz +++ b/mrpt_graphslam_2d/rviz/graphslam_real_2.rviz @@ -113,96 +113,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - back_sonar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_sonar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_plate: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: robot_model_1 Robot Description: robot_description TF Prefix: nickkoukubuntu @@ -219,96 +129,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - back_sonar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_sonar: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hokuyo_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_back_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_left_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_axle: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_hub: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - p3at_front_right_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_plate: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Name: robot_model_2 Robot Description: robot_description TF Prefix: odroidxu3 @@ -534,7 +354,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: map + Fixed Frame: mf3_anchor Frame Rate: 30 Name: root Tools: @@ -555,15 +375,15 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 20.7017 + Distance: 29.0844 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 3.048 - Y: -4.8197 + X: 5.62466 + Y: -0.580925 Z: -4.44887 Name: Current View Near Clip Distance: 0.01 @@ -578,7 +398,7 @@ Window Geometry: Height: 1027 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001a300000360fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000189000000f300fffffffb0000000a0069006d0067005f003101000001d2000000f10000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000002c9000000da0000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000bd00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000027900fffffffb0000000800540069006d00650100000000000004500000000000000000000005d70000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001a300000360fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000189000000dd00fffffffb0000000a0069006d0067005f003101000001d0000000f10000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000002c7000000da0000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d70000036000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/mrpt_graphslam_2d/rviz/templates/graphslam_bag_1.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_1.rviz new file mode 100644 index 0000000..0962004 --- /dev/null +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_1.rviz @@ -0,0 +1,245 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /map1 + - /estim_path1 + - /LaserScan1 + - /Image1 + - /Map1 + - /Axes1 + - /estim_pose1 + - /odom_path1 + Splitter Ratio: 0.5 + Tree Height: 693 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/gridmap + Unreliable: false + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: Axes + Radius: 0.1 + Reference Frame: + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 254 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/odom_trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: {HOSTNAME_PLACEHOLDER}/anchor + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 21.7711 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.921923 + Y: 0.858794 + Z: 1.06544 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.8954 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.43039 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1401 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001ee000004b9fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002c00000365000000f200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000003970000014e0000002700ffffff000000010000015b000004b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002c000004b9000000dd00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000005efc0100000002fb0000000800540069006d0065010000000000000a000000042f00fffffffb0000000800540069006d00650100000000000004500000000000000000000006ab000004b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 39 diff --git a/mrpt_graphslam_2d/rviz/templates/graphslam_bag_2.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_2.rviz new file mode 100644 index 0000000..0f0e7af --- /dev/null +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_2.rviz @@ -0,0 +1,416 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /anchor_frame_11 + - /anchor_frame_21 + - /img_21 + - /map_21 + - /odom_traj_21 + - /estim_pose_21 + - /estim_traj_21 + - /odom_traj_11 + - /estim_pose_11 + - /estim_traj_11 + Splitter Ratio: 0.5 + Tree Height: 327 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: mf1_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: mf3_anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11312/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_1 + Robot Description: robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER} + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_2 + Robot Description: robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11312 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/odom + Value: false + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_trajectory + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: {HOSTNAME_PLACEHOLDER}/anchor + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 20.7017 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.048 + Y: -4.8197 + Z: -4.44887 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.9848 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.5954 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1027 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a30000036efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000018f000000d800fffffffb0000000a0069006d0067005f003101000001bd000000f50000001a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000002b8000000de0000001a00fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e00000044fc0100000002fb0000000800540069006d006501000000000000077e000002cc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005d50000036e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: -8 + Y: 374 + img_1: + collapsed: false + img_2: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/templates/graphslam_bag_3.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_3.rviz new file mode 100644 index 0000000..f976def --- /dev/null +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_bag_3.rviz @@ -0,0 +1,552 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /img_31 + Splitter Ratio: 0.5 + Tree Height: 232 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: mf1_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: mf3_anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_3 + Radius: 0.1 + Reference Frame: mf5_anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11312/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11313/onboard_camera/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_3 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_1 + Robot Description: robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER} + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_2 + Robot Description: robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11312 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: robot_model_3 + Robot Description: robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11313 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_3 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11313/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_3 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/input/odom + Value: false + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_3 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_3 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/robot_position + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 28.726 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.92833 + Y: 3.92607 + Z: -4.21462 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 1.3298 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.77223 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a30000035cfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000129000000dd00fffffffb0000000a0069006d0067005f00310100000170000000b60000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f0032010000022c000000a50000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000fb0000000a0069006d0067005f003301000002d7000000c60000001600ffffff000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d50000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false + img_3: + collapsed: false diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_1.rviz similarity index 94% rename from mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz rename to mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_1.rviz index a2910c2..148655e 100644 --- a/mrpt_graphslam_2d/rviz/graphslam_gazebo_1.rviz +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_1.rviz @@ -64,7 +64,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map - Topic: /csldesktop/feedback/gridmap + Topic: /gc/feedback/gridmap Unreliable: false Value: true - Alpha: 1 @@ -86,7 +86,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/robot_trajectory + Topic: /gc/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 1 @@ -114,14 +114,14 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop/input/laser_scan + Topic: /gc/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop/camera1/image_raw + Image Topic: /gc/camera1/image_raw Max Value: 1 Median window: 5 Min Value: 0 @@ -137,7 +137,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: Map - Topic: /csldesktop/feedback/gridmap + Topic: /gc/feedback/gridmap Unreliable: false Value: true - Class: rviz/Axes @@ -152,7 +152,7 @@ Visualization Manager: Length: 1 Name: Axes Radius: 0.1 - Reference Frame: csldesktop_anchor + Reference Frame: gc_anchor Value: true - Alpha: 1 Axes Length: 1 @@ -166,7 +166,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop/feedback/robot_position + Topic: /gc/feedback/robot_position Unreliable: false Value: false - Alpha: 1 @@ -188,7 +188,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/odom_trajectory + Topic: /gc/feedback/odom_trajectory Unreliable: false Value: true Enabled: true diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_2.rviz similarity index 94% rename from mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz rename to mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_2.rviz index a4826ba..6298751 100644 --- a/mrpt_graphslam_2d/rviz/graphslam_gazebo_2.rviz +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_2.rviz @@ -65,18 +65,18 @@ Visualization Manager: Length: 1 Name: anchor_frame_1 Radius: 0.1 - Reference Frame: csldesktop/anchor + Reference Frame: gc/anchor Value: true - Class: rviz/Axes Enabled: true Length: 1 Name: anchor_frame_2 Radius: 0.1 - Reference Frame: csldesktop_11312/anchor + Reference Frame: gc_11312/anchor Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop/ar_sys/result + Image Topic: /gc/ar_sys/result Max Value: 1 Median window: 5 Min Value: 0 @@ -88,7 +88,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop_11312/ar_sys/result + Image Topic: /gc_11312/ar_sys/result Max Value: 1 Median window: 5 Min Value: 0 @@ -199,8 +199,8 @@ Visualization Manager: Show Trail: false Value: true Name: robot_model_1 - Robot Description: csldesktop/robot_description - TF Prefix: csldesktop + Robot Description: gc/robot_description + TF Prefix: gc Update Interval: 0 Value: true Visual Enabled: true @@ -305,8 +305,8 @@ Visualization Manager: Show Trail: false Value: true Name: robot_model_2 - Robot Description: csldesktop_11312/robot_description - TF Prefix: csldesktop_11312 + Robot Description: gc_11312/robot_description + TF Prefix: gc_11312 Update Interval: 0 Value: true Visual Enabled: true @@ -335,7 +335,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop/input/laser_scan + Topic: /gc/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -365,7 +365,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop_11312/input/laser_scan + Topic: /gc_11312/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -378,7 +378,7 @@ Visualization Manager: Length: 1 Name: odom_1 Position Tolerance: 0.1 - Topic: /csldesktop/input/odom + Topic: /gc/input/odom Value: false - Angle Tolerance: 0.1 Class: rviz/Odometry @@ -388,7 +388,7 @@ Visualization Manager: Length: 1 Name: odom_2 Position Tolerance: 0.1 - Topic: /csldesktop_11312/input/odom + Topic: /gc_11312/input/odom Value: false - Alpha: 0.7 Class: rviz/Map @@ -396,7 +396,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map_1 - Topic: /csldesktop/feedback/gridmap + Topic: /gc/feedback/gridmap Unreliable: false Value: true - Alpha: 0.7 @@ -405,7 +405,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map_2 - Topic: /csldesktop_11312/feedback/gridmap + Topic: /gc_11312/feedback/gridmap Unreliable: false Value: true - Alpha: 1 @@ -427,7 +427,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/odom_trajectory + Topic: /gc/feedback/odom_trajectory Unreliable: false Value: true - Alpha: 1 @@ -449,7 +449,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11312/feedback/odom_trajectory + Topic: /gc_11312/feedback/odom_trajectory Unreliable: false Value: true - Alpha: 1 @@ -464,7 +464,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop/feedback/robot_position + Topic: /gc/feedback/robot_position Unreliable: false Value: true - Alpha: 1 @@ -479,7 +479,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop_11312/feedback/robot_position + Topic: /gc_11312/feedback/robot_position Unreliable: false Value: true - Alpha: 1 @@ -501,7 +501,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/robot_trajectory + Topic: /gc/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 1 @@ -523,7 +523,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11312/feedback/robot_trajectory + Topic: /gc_11312/feedback/robot_trajectory Unreliable: false Value: true Enabled: true diff --git a/mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_3.rviz similarity index 93% rename from mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz rename to mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_3.rviz index eeff793..1499be8 100644 --- a/mrpt_graphslam_2d/rviz/graphslam_gazebo_3.rviz +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_3.rviz @@ -63,25 +63,25 @@ Visualization Manager: Length: 1 Name: anchor_frame_1 Radius: 0.1 - Reference Frame: csldesktop/anchor + Reference Frame: gc/anchor Value: true - Class: rviz/Axes Enabled: true Length: 1 Name: anchor_frame_2 Radius: 0.1 - Reference Frame: csldesktop_11312/anchor + Reference Frame: gc_11312/anchor Value: true - Class: rviz/Axes Enabled: true Length: 1 Name: anchor_frame_3 Radius: 0.1 - Reference Frame: csldesktop_11313/anchor + Reference Frame: gc_11313/anchor Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop/ar_sys/result + Image Topic: /gc/ar_sys/result Max Value: 1 Median window: 5 Min Value: 0 @@ -93,7 +93,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop_11312/ar_sys/result + Image Topic: /gc_11312/ar_sys/result Max Value: 1 Median window: 5 Min Value: 0 @@ -105,7 +105,7 @@ Visualization Manager: Value: true - Class: rviz/Image Enabled: true - Image Topic: /csldesktop_11313/ar_sys/result + Image Topic: /gc_11313/ar_sys/result Max Value: 1 Median window: 5 Min Value: 0 @@ -216,8 +216,8 @@ Visualization Manager: Show Trail: false Value: true Name: robot_model_1 - Robot Description: csldesktop/robot_description - TF Prefix: csldesktop + Robot Description: gc/robot_description + TF Prefix: gc Update Interval: 0 Value: true Visual Enabled: true @@ -322,8 +322,8 @@ Visualization Manager: Show Trail: false Value: true Name: robot_model_2 - Robot Description: csldesktop_11312/robot_description - TF Prefix: csldesktop_11312 + Robot Description: gc_11312/robot_description + TF Prefix: gc_11312 Update Interval: 0 Value: true Visual Enabled: true @@ -428,8 +428,8 @@ Visualization Manager: Show Trail: false Value: true Name: robot_model_3 - Robot Description: csldesktop_11313/robot_description - TF Prefix: csldesktop_11313 + Robot Description: gc_11313/robot_description + TF Prefix: gc_11313 Update Interval: 0 Value: true Visual Enabled: true @@ -458,7 +458,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop/input/laser_scan + Topic: /gc/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -488,7 +488,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop_11312/input/laser_scan + Topic: /gc_11312/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -518,7 +518,7 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /csldesktop_11313/input/laser_scan + Topic: /gc_11313/input/laser_scan Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -531,7 +531,7 @@ Visualization Manager: Length: 1 Name: odom_1 Position Tolerance: 0.1 - Topic: /csldesktop/input/odom + Topic: /gc/input/odom Value: false - Angle Tolerance: 0.1 Class: rviz/Odometry @@ -541,7 +541,7 @@ Visualization Manager: Length: 1 Name: odom_2 Position Tolerance: 0.1 - Topic: /csldesktop_11312/input/odom + Topic: /gc_11312/input/odom Value: false - Angle Tolerance: 0.1 Class: rviz/Odometry @@ -551,7 +551,7 @@ Visualization Manager: Length: 1 Name: odom_3 Position Tolerance: 0.1 - Topic: /csldesktop_11313/input/odom + Topic: /gc_11313/input/odom Value: false - Alpha: 1 Buffer Length: 1 @@ -572,7 +572,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/robot_trajectory + Topic: /gc/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 1 @@ -594,7 +594,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11312/feedback/robot_trajectory + Topic: /gc_11312/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 0.7 @@ -603,7 +603,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map_1 - Topic: /csldesktop/feedback/gridmap + Topic: /gc/feedback/gridmap Unreliable: false Value: true - Alpha: 0.7 @@ -612,7 +612,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map_3 - Topic: /csldesktop_11313/feedback/gridmap + Topic: /gc_11313/feedback/gridmap Unreliable: false Value: true - Alpha: 0.7 @@ -621,7 +621,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: map_2 - Topic: /csldesktop_11312/feedback/gridmap + Topic: /gc_11312/feedback/gridmap Unreliable: false Value: true - Alpha: 1 @@ -643,7 +643,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop/feedback/odom_trajectory + Topic: /gc/feedback/odom_trajectory Unreliable: false Value: true - Alpha: 1 @@ -665,7 +665,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11313/feedback/odom_trajectory + Topic: /gc_11313/feedback/odom_trajectory Unreliable: false Value: true - Alpha: 1 @@ -687,7 +687,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11312/feedback/odom_trajectory + Topic: /gc_11312/feedback/odom_trajectory Unreliable: false Value: true - Alpha: 1 @@ -709,7 +709,7 @@ Visualization Manager: Radius: 0.03 Shaft Diameter: 0.1 Shaft Length: 0.1 - Topic: /csldesktop_11313/feedback/robot_trajectory + Topic: /gc_11313/feedback/robot_trajectory Unreliable: false Value: true - Alpha: 1 @@ -724,7 +724,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop/feedback/robot_position + Topic: /gc/feedback/robot_position Unreliable: false Value: true - Alpha: 1 @@ -739,7 +739,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop_11313/feedback/robot_position + Topic: /gc_11313/feedback/robot_position Unreliable: false Value: true - Alpha: 1 @@ -754,7 +754,7 @@ Visualization Manager: Shaft Length: 1 Shaft Radius: 0.05 Shape: Arrow - Topic: /csldesktop_11312/feedback/robot_position + Topic: /gc_11312/feedback/robot_position Unreliable: false Value: true Enabled: true diff --git a/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_4.rviz b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_4.rviz new file mode 100644 index 0000000..f9b6fce --- /dev/null +++ b/mrpt_graphslam_2d/rviz/templates/graphslam_gazebo_4.rviz @@ -0,0 +1,1055 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 160 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: img_2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: origin_axes + Radius: 0.1 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_1 + Radius: 0.1 + Reference Frame: {HOSTNAME_PLACEHOLDER}/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_2 + Radius: 0.1 + Reference Frame: {HOSTNAME_PLACEHOLDER}_11312/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_3 + Radius: 0.1 + Reference Frame: {HOSTNAME_PLACEHOLDER}_11313/anchor + Value: true + - Class: rviz/Axes + Enabled: true + Length: 1 + Name: anchor_frame_4 + Radius: 0.1 + Reference Frame: {HOSTNAME_PLACEHOLDER}_11314/anchor + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_1 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11312/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_2 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11313/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_3 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /{HOSTNAME_PLACEHOLDER}_11314/ar_sys/result + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: img_4 + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_1 + Robot Description: {HOSTNAME_PLACEHOLDER}/robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER} + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_2 + Robot Description: {HOSTNAME_PLACEHOLDER}_11312/robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11312 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_3 + Robot Description: {HOSTNAME_PLACEHOLDER}_11313/robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11313 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + back_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + hokuyo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_back_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_axle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + p3at_front_right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top_plate: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: robot_model_4 + Robot Description: {HOSTNAME_PLACEHOLDER}_11314/robot_description + TF Prefix: {HOSTNAME_PLACEHOLDER}_11314 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_1 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_3 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11313/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: laser_scan_4 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /{HOSTNAME_PLACEHOLDER}_11314/input/laser_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_1 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_2 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_3 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/input/odom + Value: false + - Angle Tolerance: 0.1 + Class: rviz/Odometry + Color: 255; 25; 0 + Enabled: false + Keep: 100 + Length: 1 + Name: odom_4 + Position Tolerance: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11314/input/odom + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: estim_traj_4 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11314/feedback/robot_trajectory + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_2 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_3 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: map_4 + Topic: /{HOSTNAME_PLACEHOLDER}_11314/feedback/gridmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_1 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_2 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_3 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 0; 255 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: odom_traj_4 + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /{HOSTNAME_PLACEHOLDER}_11314/feedback/odom_trajectory + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_1 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_2 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11312/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_3 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11313/feedback/robot_position + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: estim_pose_4 + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Arrow + Topic: /{HOSTNAME_PLACEHOLDER}_11314/feedback/robot_position + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 18.9857 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 4.27584 + Y: -5.9404 + Z: -3.04498 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.7648 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.75222 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1023 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001a30000035cfc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000041000000e1000000dd00fffffffb0000000a0069006d0067005f003101000001280000008a0000001600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0069006d0067005f003201000001b80000007d0000001600fffffffb0000000a0069006d0067005f003101000001d7000000b20000000000000000fb0000000a0069006d0067005f0033010000023b000000960000001600fffffffb0000000a0069006d0067005f003401000002d7000000c60000001600ffffff000000010000010f000001e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000001e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e0000028400fffffffb0000000800540069006d00650100000000000004500000000000000000000005d50000035c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1918 + X: 0 + Y: 31 + img_1: + collapsed: false + img_2: + collapsed: false + img_3: + collapsed: false + img_4: + collapsed: false