Renames google_cartographer* to cartographer_ros*.
parent
63c2321d26
commit
aabd51e029
|
@ -14,12 +14,12 @@
|
||||||
|
|
||||||
cmake_minimum_required(VERSION 2.8)
|
cmake_minimum_required(VERSION 2.8)
|
||||||
|
|
||||||
project(google_cartographer)
|
project(cartographer_ros)
|
||||||
|
|
||||||
set(PACKAGE_DEPENDENCIES
|
set(PACKAGE_DEPENDENCIES
|
||||||
eigen_conversions
|
eigen_conversions
|
||||||
geometry_msgs
|
geometry_msgs
|
||||||
google_cartographer_msgs
|
cartographer_ros_msgs
|
||||||
roscpp
|
roscpp
|
||||||
rviz
|
rviz
|
||||||
sensor_msgs
|
sensor_msgs
|
|
@ -75,18 +75,18 @@
|
||||||
</node>
|
</node>
|
||||||
|
|
||||||
<param name="robot_description"
|
<param name="robot_description"
|
||||||
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
|
textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />
|
||||||
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||||
type="robot_state_publisher" />
|
type="robot_state_publisher" />
|
||||||
|
|
||||||
<node name="cartographer" pkg="google_cartographer"
|
<node name="cartographer" pkg="cartographer_ros"
|
||||||
type="cartographer_node" output="screen" >
|
type="cartographer_node" output="screen" >
|
||||||
<rosparam subst_value="true">
|
<rosparam subst_value="true">
|
||||||
map_frame: "map"
|
map_frame: "map"
|
||||||
tracking_frame: "base_link"
|
tracking_frame: "base_link"
|
||||||
configuration_files_directories: [
|
configuration_files_directories: [
|
||||||
"$(find google_cartographer)/configuration_files"
|
"$(find cartographer_ros)/configuration_files"
|
||||||
]
|
]
|
||||||
mapping_configuration_basename: "3d_mapping.lua"
|
mapping_configuration_basename: "3d_mapping.lua"
|
||||||
imu_topic: "/imu"
|
imu_topic: "/imu"
|
|
@ -16,12 +16,12 @@
|
||||||
|
|
||||||
<launch>
|
<launch>
|
||||||
<param name="robot_description"
|
<param name="robot_description"
|
||||||
textfile="$(find google_cartographer)/urdf/backpack_3d.urdf" />
|
textfile="$(find cartographer_ros)/urdf/backpack_3d.urdf" />
|
||||||
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||||
type="robot_state_publisher" />
|
type="robot_state_publisher" />
|
||||||
|
|
||||||
<node name="cartographer" pkg="google_cartographer"
|
<node name="cartographer" pkg="cartographer_ros"
|
||||||
type="cartographer_node" output="screen" >
|
type="cartographer_node" output="screen" >
|
||||||
<rosparam subst_value="true">
|
<rosparam subst_value="true">
|
||||||
# This node publishes the transformation between the tracking and map
|
# This node publishes the transformation between the tracking and map
|
||||||
|
@ -35,7 +35,7 @@
|
||||||
# want to add your own configuration overwrites in your own node
|
# want to add your own configuration overwrites in your own node
|
||||||
# directories - you should add the path here as first entry then.
|
# directories - you should add the path here as first entry then.
|
||||||
configuration_files_directories: [
|
configuration_files_directories: [
|
||||||
"$(find google_cartographer)/configuration_files"
|
"$(find cartographer_ros)/configuration_files"
|
||||||
]
|
]
|
||||||
|
|
||||||
# Configuration file for SLAM. The settings in here are tweaked to the
|
# Configuration file for SLAM. The settings in here are tweaked to the
|
|
@ -16,18 +16,18 @@
|
||||||
|
|
||||||
<launch>
|
<launch>
|
||||||
<param name="robot_description"
|
<param name="robot_description"
|
||||||
textfile="$(find google_cartographer)/urdf/turtlebot.urdf" />
|
textfile="$(find cartographer_ros)/urdf/turtlebot.urdf" />
|
||||||
|
|
||||||
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
<node name="robot_state_publisher" pkg="robot_state_publisher"
|
||||||
type="robot_state_publisher" />
|
type="robot_state_publisher" />
|
||||||
|
|
||||||
<node name="cartographer" pkg="google_cartographer"
|
<node name="cartographer" pkg="cartographer_ros"
|
||||||
type="cartographer_node" output="screen" >
|
type="cartographer_node" output="screen" >
|
||||||
<rosparam subst_value="true">
|
<rosparam subst_value="true">
|
||||||
map_frame: "map"
|
map_frame: "map"
|
||||||
tracking_frame: "base_link"
|
tracking_frame: "base_link"
|
||||||
configuration_files_directories: [
|
configuration_files_directories: [
|
||||||
"$(find google_cartographer)/configuration_files"
|
"$(find cartographer_ros)/configuration_files"
|
||||||
]
|
]
|
||||||
mapping_configuration_basename: "turtlebot.lua"
|
mapping_configuration_basename: "turtlebot.lua"
|
||||||
imu_topic: "/imu"
|
imu_topic: "/imu"
|
|
@ -14,22 +14,22 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
fragment_program google_cartographer/glsl120/submap.frag glsl
|
fragment_program cartographer_ros/glsl120/submap.frag glsl
|
||||||
{
|
{
|
||||||
source submap.frag
|
source submap.frag
|
||||||
}
|
}
|
||||||
|
|
||||||
vertex_program google_cartographer/glsl120/submap.vert glsl
|
vertex_program cartographer_ros/glsl120/submap.vert glsl
|
||||||
{
|
{
|
||||||
source submap.vert
|
source submap.vert
|
||||||
}
|
}
|
||||||
|
|
||||||
fragment_program google_cartographer/glsl120/screen_blit.frag glsl
|
fragment_program cartographer_ros/glsl120/screen_blit.frag glsl
|
||||||
{
|
{
|
||||||
source screen_blit.frag
|
source screen_blit.frag
|
||||||
}
|
}
|
||||||
|
|
||||||
vertex_program google_cartographer/glsl120/screen_blit.vert glsl
|
vertex_program cartographer_ros/glsl120/screen_blit.vert glsl
|
||||||
{
|
{
|
||||||
source screen_blit.vert
|
source screen_blit.vert
|
||||||
}
|
}
|
|
@ -14,15 +14,15 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
material google_cartographer/ScreenBlit
|
material cartographer_ros/ScreenBlit
|
||||||
{
|
{
|
||||||
technique
|
technique
|
||||||
{
|
{
|
||||||
pass
|
pass
|
||||||
{
|
{
|
||||||
vertex_program_ref google_cartographer/glsl120/screen_blit.vert {}
|
vertex_program_ref cartographer_ros/glsl120/screen_blit.vert {}
|
||||||
|
|
||||||
fragment_program_ref google_cartographer/glsl120/screen_blit.frag
|
fragment_program_ref cartographer_ros/glsl120/screen_blit.frag
|
||||||
{
|
{
|
||||||
param_named u_texture int 0
|
param_named u_texture int 0
|
||||||
}
|
}
|
|
@ -14,15 +14,15 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
material google_cartographer/Submap
|
material cartographer_ros/Submap
|
||||||
{
|
{
|
||||||
technique
|
technique
|
||||||
{
|
{
|
||||||
pass
|
pass
|
||||||
{
|
{
|
||||||
vertex_program_ref google_cartographer/glsl120/submap.vert {}
|
vertex_program_ref cartographer_ros/glsl120/submap.vert {}
|
||||||
|
|
||||||
fragment_program_ref google_cartographer/glsl120/submap.frag
|
fragment_program_ref cartographer_ros/glsl120/submap.frag
|
||||||
{
|
{
|
||||||
param_named u_submap int 0
|
param_named u_submap int 0
|
||||||
param_named u_alpha float 1.0
|
param_named u_alpha float 1.0
|
|
@ -16,9 +16,9 @@
|
||||||
-->
|
-->
|
||||||
|
|
||||||
<package>
|
<package>
|
||||||
<name>google_cartographer</name>
|
<name>cartographer_ros</name>
|
||||||
<version>1.0.0</version>
|
<version>1.0.0</version>
|
||||||
<description>The google_cartographer package.</description>
|
<description>The cartographer_ros package.</description>
|
||||||
<maintainer email="nobody@google.com">Google</maintainer>
|
<maintainer email="nobody@google.com">Google</maintainer>
|
||||||
<license>Apache 2.0</license>
|
<license>Apache 2.0</license>
|
||||||
|
|
||||||
|
@ -26,7 +26,7 @@
|
||||||
|
|
||||||
<build_depend>eigen_conversions</build_depend>
|
<build_depend>eigen_conversions</build_depend>
|
||||||
<build_depend>geometry_msgs</build_depend>
|
<build_depend>geometry_msgs</build_depend>
|
||||||
<build_depend>google_cartographer_msgs</build_depend>
|
<build_depend>cartographer_ros_msgs</build_depend>
|
||||||
<build_depend>libpcl-all-dev</build_depend>
|
<build_depend>libpcl-all-dev</build_depend>
|
||||||
<build_depend>message_generation</build_depend>
|
<build_depend>message_generation</build_depend>
|
||||||
<build_depend>nav_msgs</build_depend>
|
<build_depend>nav_msgs</build_depend>
|
||||||
|
@ -44,7 +44,7 @@
|
||||||
|
|
||||||
<run_depend>eigen_conversions</run_depend>
|
<run_depend>eigen_conversions</run_depend>
|
||||||
<run_depend>geometry_msgs</run_depend>
|
<run_depend>geometry_msgs</run_depend>
|
||||||
<run_depend>google_cartographer_msgs</run_depend>
|
<run_depend>cartographer_ros_msgs</run_depend>
|
||||||
<run_depend>libpcl-all</run_depend>
|
<run_depend>libpcl-all</run_depend>
|
||||||
<run_depend>libqt5-core</run_depend>
|
<run_depend>libqt5-core</run_depend>
|
||||||
<run_depend>libqt5-gui</run_depend>
|
<run_depend>libqt5-gui</run_depend>
|
|
@ -46,10 +46,10 @@
|
||||||
#include "geometry_msgs/TransformStamped.h"
|
#include "geometry_msgs/TransformStamped.h"
|
||||||
#include "glog/log_severity.h"
|
#include "glog/log_severity.h"
|
||||||
#include "glog/logging.h"
|
#include "glog/logging.h"
|
||||||
#include "google_cartographer_msgs/SubmapEntry.h"
|
#include "cartographer_ros_msgs/SubmapEntry.h"
|
||||||
#include "google_cartographer_msgs/SubmapList.h"
|
#include "cartographer_ros_msgs/SubmapList.h"
|
||||||
#include "google_cartographer_msgs/SubmapQuery.h"
|
#include "cartographer_ros_msgs/SubmapQuery.h"
|
||||||
#include "google_cartographer_msgs/TrajectorySubmapList.h"
|
#include "cartographer_ros_msgs/TrajectorySubmapList.h"
|
||||||
#include "pcl/point_cloud.h"
|
#include "pcl/point_cloud.h"
|
||||||
#include "pcl/point_types.h"
|
#include "pcl/point_types.h"
|
||||||
#include "pcl_conversions/pcl_conversions.h"
|
#include "pcl_conversions/pcl_conversions.h"
|
||||||
|
@ -173,8 +173,8 @@ class Node {
|
||||||
const string& frame_id);
|
const string& frame_id);
|
||||||
|
|
||||||
bool HandleSubmapQuery(
|
bool HandleSubmapQuery(
|
||||||
::google_cartographer_msgs::SubmapQuery::Request& request,
|
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
::google_cartographer_msgs::SubmapQuery::Response& response);
|
::cartographer_ros_msgs::SubmapQuery::Response& response);
|
||||||
|
|
||||||
void PublishSubmapList(int64 timestamp);
|
void PublishSubmapList(int64 timestamp);
|
||||||
void PublishPose(int64 timestamp);
|
void PublishPose(int64 timestamp);
|
||||||
|
@ -441,15 +441,15 @@ void Node::Initialize() {
|
||||||
});
|
});
|
||||||
|
|
||||||
submap_list_publisher_ =
|
submap_list_publisher_ =
|
||||||
node_handle_.advertise<::google_cartographer_msgs::SubmapList>(
|
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
|
||||||
kSubmapListTopic, 10);
|
kSubmapListTopic, 10);
|
||||||
submap_query_server_ = node_handle_.advertiseService(
|
submap_query_server_ = node_handle_.advertiseService(
|
||||||
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
|
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool Node::HandleSubmapQuery(
|
bool Node::HandleSubmapQuery(
|
||||||
::google_cartographer_msgs::SubmapQuery::Request& request,
|
::cartographer_ros_msgs::SubmapQuery::Request& request,
|
||||||
::google_cartographer_msgs::SubmapQuery::Response& response) {
|
::cartographer_ros_msgs::SubmapQuery::Response& response) {
|
||||||
if (request.trajectory_id != 0) {
|
if (request.trajectory_id != 0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -506,15 +506,15 @@ void Node::PublishSubmapList(int64 timestamp) {
|
||||||
sparse_pose_graph_->GetSubmapTransforms(*submaps);
|
sparse_pose_graph_->GetSubmapTransforms(*submaps);
|
||||||
CHECK_EQ(submap_transforms.size(), submaps->size());
|
CHECK_EQ(submap_transforms.size(), submaps->size());
|
||||||
|
|
||||||
::google_cartographer_msgs::TrajectorySubmapList ros_trajectory;
|
::cartographer_ros_msgs::TrajectorySubmapList ros_trajectory;
|
||||||
for (int i = 0; i != submaps->size(); ++i) {
|
for (int i = 0; i != submaps->size(); ++i) {
|
||||||
::google_cartographer_msgs::SubmapEntry ros_submap;
|
::cartographer_ros_msgs::SubmapEntry ros_submap;
|
||||||
ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index;
|
ros_submap.submap_version = submaps->Get(i)->end_laser_fan_index;
|
||||||
ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]);
|
ros_submap.pose = ToGeometryMsgPose(submap_transforms[i]);
|
||||||
ros_trajectory.submap.push_back(ros_submap);
|
ros_trajectory.submap.push_back(ros_submap);
|
||||||
}
|
}
|
||||||
|
|
||||||
::google_cartographer_msgs::SubmapList ros_submap_list;
|
::cartographer_ros_msgs::SubmapList ros_submap_list;
|
||||||
ros_submap_list.trajectory.push_back(ros_trajectory);
|
ros_submap_list.trajectory.push_back(ros_trajectory);
|
||||||
submap_list_publisher_.publish(ros_submap_list);
|
submap_list_publisher_.publish(ros_submap_list);
|
||||||
last_submap_list_publish_timestamp_ = timestamp;
|
last_submap_list_publish_timestamp_ = timestamp;
|
|
@ -20,7 +20,7 @@
|
||||||
#include <OgreImage.h>
|
#include <OgreImage.h>
|
||||||
#include <cartographer/common/port.h>
|
#include <cartographer/common/port.h>
|
||||||
#include <eigen_conversions/eigen_msg.h>
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
#include <cartographer_ros_msgs/SubmapQuery.h>
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <rviz/display_context.h>
|
#include <rviz/display_context.h>
|
||||||
#include <rviz/frame_manager.h>
|
#include <rviz/frame_manager.h>
|
||||||
|
@ -42,7 +42,7 @@ constexpr char kMapFrame[] = "/map";
|
||||||
constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
|
constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
|
||||||
constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
|
constexpr char kManualObjectPrefix[] = "ManualObjectSubmap";
|
||||||
constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
|
constexpr char kSubmapMaterialPrefix[] = "SubmapMaterial";
|
||||||
constexpr char kSubmapSourceMaterialName[] = "google_cartographer/Submap";
|
constexpr char kSubmapSourceMaterialName[] = "cartographer_ros/Submap";
|
||||||
|
|
||||||
// Distance before which the submap will be shown at full opacity, and distance
|
// Distance before which the submap will be shown at full opacity, and distance
|
||||||
// over which the submap will then fade out.
|
// over which the submap will then fade out.
|
||||||
|
@ -89,7 +89,7 @@ DrawableSubmap::~DrawableSubmap() {
|
||||||
}
|
}
|
||||||
|
|
||||||
bool DrawableSubmap::Update(
|
bool DrawableSubmap::Update(
|
||||||
const ::google_cartographer_msgs::SubmapEntry& metadata,
|
const ::cartographer_ros_msgs::SubmapEntry& metadata,
|
||||||
ros::ServiceClient* const client) {
|
ros::ServiceClient* const client) {
|
||||||
::cartographer::common::MutexLocker locker(&mutex_);
|
::cartographer::common::MutexLocker locker(&mutex_);
|
||||||
tf::poseMsgToEigen(metadata.pose, submap_pose_);
|
tf::poseMsgToEigen(metadata.pose, submap_pose_);
|
||||||
|
@ -113,11 +113,11 @@ void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
|
||||||
ros::ServiceClient* const client) {
|
ros::ServiceClient* const client) {
|
||||||
rpc_request_future_ = std::async(
|
rpc_request_future_ = std::async(
|
||||||
std::launch::async, [this, submap_id, trajectory_id, client]() {
|
std::launch::async, [this, submap_id, trajectory_id, client]() {
|
||||||
::google_cartographer_msgs::SubmapQuery srv;
|
::cartographer_ros_msgs::SubmapQuery srv;
|
||||||
srv.request.submap_id = submap_id;
|
srv.request.submap_id = submap_id;
|
||||||
srv.request.trajectory_id = trajectory_id;
|
srv.request.trajectory_id = trajectory_id;
|
||||||
if (client->call(srv)) {
|
if (client->call(srv)) {
|
||||||
response_.reset(new ::google_cartographer_msgs::SubmapQuery::Response(
|
response_.reset(new ::cartographer_ros_msgs::SubmapQuery::Response(
|
||||||
srv.response));
|
srv.response));
|
||||||
Q_EMIT RequestSucceeded();
|
Q_EMIT RequestSucceeded();
|
||||||
} else {
|
} else {
|
|
@ -23,8 +23,8 @@
|
||||||
#include <OgreSceneNode.h>
|
#include <OgreSceneNode.h>
|
||||||
#include <OgreTexture.h>
|
#include <OgreTexture.h>
|
||||||
#include <cartographer/common/mutex.h>
|
#include <cartographer/common/mutex.h>
|
||||||
#include <google_cartographer_msgs/SubmapEntry.h>
|
#include <cartographer_ros_msgs/SubmapEntry.h>
|
||||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
#include <cartographer_ros_msgs/SubmapQuery.h>
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
#include <rviz/display_context.h>
|
#include <rviz/display_context.h>
|
||||||
#include <Eigen/Core>
|
#include <Eigen/Core>
|
||||||
|
@ -55,7 +55,7 @@ class DrawableSubmap : public QObject {
|
||||||
// 'submap_entry' contains metadata which is used to find out whether this
|
// 'submap_entry' contains metadata which is used to find out whether this
|
||||||
// 'DrawableSubmap' should update itself. If an update is needed, it will send
|
// 'DrawableSubmap' should update itself. If an update is needed, it will send
|
||||||
// an RPC using 'client' to request the new data for the submap.
|
// an RPC using 'client' to request the new data for the submap.
|
||||||
bool Update(const ::google_cartographer_msgs::SubmapEntry& submap_entry,
|
bool Update(const ::cartographer_ros_msgs::SubmapEntry& submap_entry,
|
||||||
ros::ServiceClient* client);
|
ros::ServiceClient* client);
|
||||||
|
|
||||||
// Returns whether an RPC is in progress.
|
// Returns whether an RPC is in progress.
|
||||||
|
@ -105,7 +105,7 @@ class DrawableSubmap : public QObject {
|
||||||
double last_query_slice_height_ GUARDED_BY(mutex_);
|
double last_query_slice_height_ GUARDED_BY(mutex_);
|
||||||
std::future<void> rpc_request_future_;
|
std::future<void> rpc_request_future_;
|
||||||
std::string cells_ GUARDED_BY(mutex_);
|
std::string cells_ GUARDED_BY(mutex_);
|
||||||
std::unique_ptr<::google_cartographer_msgs::SubmapQuery::Response> response_
|
std::unique_ptr<::cartographer_ros_msgs::SubmapQuery::Response> response_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
int texture_count_;
|
int texture_count_;
|
||||||
float current_alpha_;
|
float current_alpha_;
|
|
@ -35,8 +35,8 @@
|
||||||
#include <OgreViewport.h>
|
#include <OgreViewport.h>
|
||||||
#include <cartographer/common/mutex.h>
|
#include <cartographer/common/mutex.h>
|
||||||
#include <geometry_msgs/TransformStamped.h>
|
#include <geometry_msgs/TransformStamped.h>
|
||||||
#include <google_cartographer_msgs/SubmapList.h>
|
#include <cartographer_ros_msgs/SubmapList.h>
|
||||||
#include <google_cartographer_msgs/SubmapQuery.h>
|
#include <cartographer_ros_msgs/SubmapQuery.h>
|
||||||
#include <pluginlib/class_list_macros.h>
|
#include <pluginlib/class_list_macros.h>
|
||||||
#include <ros/package.h>
|
#include <ros/package.h>
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
@ -56,7 +56,7 @@ constexpr char kGlsl120Directory[] = "/glsl120";
|
||||||
constexpr char kScriptsDirectory[] = "/scripts";
|
constexpr char kScriptsDirectory[] = "/scripts";
|
||||||
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
|
constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
|
||||||
constexpr char kScreenBlitSourceMaterialName[] =
|
constexpr char kScreenBlitSourceMaterialName[] =
|
||||||
"google_cartographer/ScreenBlit";
|
"cartographer_ros/ScreenBlit";
|
||||||
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
|
constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
|
||||||
constexpr char kMapTextureName[] = "MapTexture";
|
constexpr char kMapTextureName[] = "MapTexture";
|
||||||
constexpr char kMapOverlayName[] = "MapOverlay";
|
constexpr char kMapOverlayName[] = "MapOverlay";
|
||||||
|
@ -76,8 +76,8 @@ SubmapsDisplay::SubmapsDisplay()
|
||||||
topic_property_ = new ::rviz::RosTopicProperty(
|
topic_property_ = new ::rviz::RosTopicProperty(
|
||||||
"Topic", "",
|
"Topic", "",
|
||||||
QString::fromStdString(ros::message_traits::datatype<
|
QString::fromStdString(ros::message_traits::datatype<
|
||||||
::google_cartographer_msgs::SubmapList>()),
|
::cartographer_ros_msgs::SubmapList>()),
|
||||||
"google_cartographer_msgs::SubmapList topic to subscribe to.", this,
|
"cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
|
||||||
SLOT(UpdateTopic()));
|
SLOT(UpdateTopic()));
|
||||||
submap_query_service_property_ = new ::rviz::StringProperty(
|
submap_query_service_property_ = new ::rviz::StringProperty(
|
||||||
"Submap query service", "", "Submap query service to connect to.", this,
|
"Submap query service", "", "Submap query service to connect to.", this,
|
||||||
|
@ -89,7 +89,7 @@ SubmapsDisplay::SubmapsDisplay()
|
||||||
"Tracking frame", kDefaultTrackingFrame,
|
"Tracking frame", kDefaultTrackingFrame,
|
||||||
"Tracking frame, used for fading out submaps.", this);
|
"Tracking frame, used for fading out submaps.", this);
|
||||||
client_ =
|
client_ =
|
||||||
update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>("");
|
update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
|
||||||
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
|
const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
|
||||||
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
|
||||||
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
|
package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
|
||||||
|
@ -149,7 +149,7 @@ void SubmapsDisplay::UpdateSubmapQueryServiceName() {
|
||||||
Unsubscribe();
|
Unsubscribe();
|
||||||
Clear();
|
Clear();
|
||||||
client_.shutdown();
|
client_.shutdown();
|
||||||
client_ = update_nh_.serviceClient<::google_cartographer_msgs::SubmapQuery>(
|
client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
|
||||||
submap_query_service_property_->getStdString());
|
submap_query_service_property_->getStdString());
|
||||||
Subscribe();
|
Subscribe();
|
||||||
}
|
}
|
||||||
|
@ -190,7 +190,7 @@ void SubmapsDisplay::Subscribe() {
|
||||||
void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
|
void SubmapsDisplay::Unsubscribe() { submap_list_subscriber_.shutdown(); }
|
||||||
|
|
||||||
void SubmapsDisplay::IncomingSubmapList(
|
void SubmapsDisplay::IncomingSubmapList(
|
||||||
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg) {
|
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
|
||||||
submap_list_ = *msg;
|
submap_list_ = *msg;
|
||||||
Q_EMIT SubmapListUpdated();
|
Q_EMIT SubmapListUpdated();
|
||||||
}
|
}
|
||||||
|
@ -215,7 +215,7 @@ void SubmapsDisplay::RequestNewSubmaps() {
|
||||||
if (trajectory_id >= trajectories_.size()) {
|
if (trajectory_id >= trajectories_.size()) {
|
||||||
trajectories_.emplace_back(new Trajectory);
|
trajectories_.emplace_back(new Trajectory);
|
||||||
}
|
}
|
||||||
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
|
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
|
||||||
submap_list_.trajectory[trajectory_id].submap;
|
submap_list_.trajectory[trajectory_id].submap;
|
||||||
if (submap_entries.empty()) {
|
if (submap_entries.empty()) {
|
||||||
return;
|
return;
|
||||||
|
@ -240,7 +240,7 @@ void SubmapsDisplay::RequestNewSubmaps() {
|
||||||
}
|
}
|
||||||
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
|
for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
|
||||||
++trajectory_id) {
|
++trajectory_id) {
|
||||||
const std::vector<::google_cartographer_msgs::SubmapEntry>& submap_entries =
|
const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
|
||||||
submap_list_.trajectory[trajectory_id].submap;
|
submap_list_.trajectory[trajectory_id].submap;
|
||||||
for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
|
for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
|
||||||
--submap_id) {
|
--submap_id) {
|
|
@ -26,7 +26,7 @@
|
||||||
#include <OgreVector3.h>
|
#include <OgreVector3.h>
|
||||||
#include <cartographer/common/mutex.h>
|
#include <cartographer/common/mutex.h>
|
||||||
#include <cartographer/common/port.h>
|
#include <cartographer/common/port.h>
|
||||||
#include <google_cartographer_msgs/SubmapList.h>
|
#include <cartographer_ros_msgs/SubmapList.h>
|
||||||
#include <nav_msgs/MapMetaData.h>
|
#include <nav_msgs/MapMetaData.h>
|
||||||
#include <ros/time.h>
|
#include <ros/time.h>
|
||||||
#include <rviz/display.h>
|
#include <rviz/display.h>
|
||||||
|
@ -94,14 +94,14 @@ class SubmapsDisplay : public ::rviz::Display {
|
||||||
void Unsubscribe();
|
void Unsubscribe();
|
||||||
void UpdateMapTexture();
|
void UpdateMapTexture();
|
||||||
void IncomingSubmapList(
|
void IncomingSubmapList(
|
||||||
const ::google_cartographer_msgs::SubmapList::ConstPtr& msg);
|
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg);
|
||||||
// Clears the current map.
|
// Clears the current map.
|
||||||
void Clear();
|
void Clear();
|
||||||
void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
|
void UpdateCurrentTrackingZ(const tf::tfMessage::ConstPtr& msg);
|
||||||
|
|
||||||
int rtt_count_;
|
int rtt_count_;
|
||||||
SceneManagerListener scene_manager_listener_;
|
SceneManagerListener scene_manager_listener_;
|
||||||
::google_cartographer_msgs::SubmapList submap_list_;
|
::cartographer_ros_msgs::SubmapList submap_list_;
|
||||||
ros::Subscriber submap_list_subscriber_;
|
ros::Subscriber submap_list_subscriber_;
|
||||||
::tf2_ros::Buffer tf_buffer_;
|
::tf2_ros::Buffer tf_buffer_;
|
||||||
::tf2_ros::TransformListener tf_listener_;
|
::tf2_ros::TransformListener tf_listener_;
|
|
@ -14,7 +14,7 @@
|
||||||
limitations under the License.
|
limitations under the License.
|
||||||
-->
|
-->
|
||||||
|
|
||||||
<robot name="google_cartographer">
|
<robot name="cartographer_ros">
|
||||||
<material name="orange">
|
<material name="orange">
|
||||||
<color rgba="1.0 0.5 0.2 1"/>
|
<color rgba="1.0 0.5 0.2 1"/>
|
||||||
</material>
|
</material>
|
|
@ -14,7 +14,7 @@
|
||||||
limitations under the License.
|
limitations under the License.
|
||||||
-->
|
-->
|
||||||
|
|
||||||
<robot name="google_cartographer">
|
<robot name="cartographer_ros">
|
||||||
<material name="orange">
|
<material name="orange">
|
||||||
<color rgba="1.0 0.5 0.2 1"/>
|
<color rgba="1.0 0.5 0.2 1"/>
|
||||||
</material>
|
</material>
|
|
@ -14,7 +14,7 @@
|
||||||
limitations under the License.
|
limitations under the License.
|
||||||
-->
|
-->
|
||||||
|
|
||||||
<robot name="google_cartographer">
|
<robot name="cartographer_ros">
|
||||||
<material name="orange">
|
<material name="orange">
|
||||||
<color rgba="1.0 0.5 0.2 1"/>
|
<color rgba="1.0 0.5 0.2 1"/>
|
||||||
</material>
|
</material>
|
|
@ -13,7 +13,7 @@
|
||||||
# limitations under the License.
|
# limitations under the License.
|
||||||
|
|
||||||
cmake_minimum_required(VERSION 2.8.3)
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
project(google_cartographer_msgs)
|
project(cartographer_ros_msgs)
|
||||||
set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
|
set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}")
|
||||||
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
|
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation)
|
||||||
|
|
|
@ -15,10 +15,10 @@
|
||||||
-->
|
-->
|
||||||
|
|
||||||
<package>
|
<package>
|
||||||
<name>google_cartographer_msgs</name>
|
<name>cartographer_ros_msgs</name>
|
||||||
<version>1.0.0</version>
|
<version>1.0.0</version>
|
||||||
<description>
|
<description>
|
||||||
The google_cartographer_msgs package.
|
The cartographer_ros_msgs package.
|
||||||
</description>
|
</description>
|
||||||
<maintainer email="nobody@google.com">Google</maintainer>
|
<maintainer email="nobody@google.com">Google</maintainer>
|
||||||
<license>Apache 2.0</license>
|
<license>Apache 2.0</license>
|
Loading…
Reference in New Issue