diff --git a/cartographer_ros/cartographer_ros/CMakeLists.txt b/cartographer_ros/cartographer_ros/CMakeLists.txt index cd1203b..7c17bf9 100644 --- a/cartographer_ros/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/cartographer_ros/CMakeLists.txt @@ -17,6 +17,12 @@ google_binary(cartographer_assets_writer assets_writer_main.cc ) +install(TARGETS cartographer_assets_writer + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + google_binary(cartographer_node SRCS node_main.cc @@ -28,12 +34,6 @@ install(TARGETS cartographer_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(TARGETS cartographer_assets_writer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - google_binary(cartographer_offline_node SRCS offline_node_main.cc @@ -44,3 +44,15 @@ install(TARGETS cartographer_offline_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + +google_binary(cartographer_start_trajectory + SRCS + start_trajectory_main.cc +) + +install(TARGETS cartographer_start_trajectory + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index f480f51..f7b9c47 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -53,25 +53,6 @@ namespace { constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kLatestOnlyPublisherQueueSize = 1; -// Try to convert 'msg' into 'options'. Returns false on failure. -bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, - TrajectoryOptions* options) { - options->tracking_frame = msg.tracking_frame; - options->published_frame = msg.published_frame; - options->odom_frame = msg.odom_frame; - options->provide_odom_frame = msg.provide_odom_frame; - options->use_odometry = msg.use_odometry; - options->use_laser_scan = msg.use_laser_scan; - options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan; - options->num_point_clouds = msg.num_point_clouds; - if (!options->trajectory_builder_options.ParseFromString( - msg.trajectory_builder_options_proto)) { - LOG(ERROR) << "Failed to parse protobuf"; - return false; - } - return true; -} - void ShutdownSubscriber(std::unordered_map& subscribers, int trajectory_id) { if (subscribers.count(trajectory_id) == 0) { diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 7dc792a..8905b31 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -22,6 +22,7 @@ #include "cartographer/common/mutex.h" #include "cartographer_ros/map_builder_bridge.h" +#include "cartographer_ros/node_constants.h" #include "cartographer_ros/node_options.h" #include "cartographer_ros/trajectory_options.h" #include "cartographer_ros_msgs/FinishTrajectory.h" @@ -37,21 +38,6 @@ namespace cartographer_ros { -// Default topic names; expected to be remapped as needed. -constexpr char kLaserScanTopic[] = "scan"; -constexpr char kMultiEchoLaserScanTopic[] = "echoes"; -constexpr char kPointCloud2Topic[] = "points2"; -constexpr char kImuTopic[] = "imu"; -constexpr char kOdometryTopic[] = "odom"; -constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; -constexpr char kOccupancyGridTopic[] = "map"; -constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; -constexpr char kSubmapListTopic[] = "submap_list"; -constexpr char kSubmapQueryServiceName[] = "submap_query"; -constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; -constexpr char kWriteAssetsServiceName[] = "write_assets"; -constexpr char kTrajectoryNodesListTopic[] = "trajectory_nodes_list"; - // Wires up ROS topics to SLAM. class Node { public: diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h new file mode 100644 index 0000000..da7c73e --- /dev/null +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -0,0 +1,39 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ +#define CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ + +namespace cartographer_ros { + +// Default topic names; expected to be remapped as needed. +constexpr char kLaserScanTopic[] = "scan"; +constexpr char kMultiEchoLaserScanTopic[] = "echoes"; +constexpr char kPointCloud2Topic[] = "points2"; +constexpr char kImuTopic[] = "imu"; +constexpr char kOdometryTopic[] = "odom"; +constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory"; +constexpr char kOccupancyGridTopic[] = "map"; +constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2"; +constexpr char kSubmapListTopic[] = "submap_list"; +constexpr char kSubmapQueryServiceName[] = "submap_query"; +constexpr char kStartTrajectoryServiceName[] = "start_trajectory"; +constexpr char kWriteAssetsServiceName[] = "write_assets"; +constexpr char kTrajectoryNodesListTopic[] = "trajectory_nodes_list"; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc new file mode 100644 index 0000000..7f81969 --- /dev/null +++ b/cartographer_ros/cartographer_ros/start_trajectory_main.cc @@ -0,0 +1,100 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include "cartographer/common/configuration_file_resolver.h" +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/make_unique.h" +#include "cartographer/common/port.h" +#include "cartographer_ros/node_constants.h" +#include "cartographer_ros/ros_log_sink.h" +#include "cartographer_ros/trajectory_options.h" +#include "cartographer_ros_msgs/StartTrajectory.h" +#include "cartographer_ros_msgs/TrajectoryOptions.h" +#include "gflags/gflags.h" +#include "ros/ros.h" + +DEFINE_string(configuration_directory, "", + "First directory in which configuration files are searched, " + "second is always the Cartographer installation to allow " + "including files from there."); +DEFINE_string(configuration_basename, "", + "Basename, i.e. not containing any directory prefix, of the " + "configuration file."); + +namespace cartographer_ros { +namespace { + +TrajectoryOptions LoadOptions() { + auto file_resolver = cartographer::common::make_unique< + cartographer::common::ConfigurationFileResolver>( + std::vector{FLAGS_configuration_directory}); + const string code = + file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); + auto lua_parameter_dictionary = + cartographer::common::LuaParameterDictionary::NonReferenceCounted( + code, std::move(file_resolver)); + return CreateTrajectoryOptions(lua_parameter_dictionary.get()); +} + +bool Run() { + ros::NodeHandle node_handle; + ros::ServiceClient client = + node_handle.serviceClient( + kStartTrajectoryServiceName); + cartographer_ros_msgs::StartTrajectory srv; + srv.request.options = ToRosMessage(LoadOptions()); + srv.request.topics.laser_scan_topic = kLaserScanTopic; + srv.request.topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; + srv.request.topics.point_cloud2_topic = kPointCloud2Topic; + srv.request.topics.imu_topic = kImuTopic; + srv.request.topics.odometry_topic = kOdometryTopic; + + if (!client.call(srv)) { + LOG(ERROR) << "Error starting trajectory."; + return false; + } + LOG(INFO) << "Started trajectory " << srv.response.trajectory_id; + return true; +} + +} // namespace +} // namespace cartographer_ros + +int main(int argc, char** argv) { + google::InitGoogleLogging(argv[0]); + google::SetUsageMessage( + "\n\n" + "Convenience tool around the start_trajectory service. This takes a Lua " + "file that is accepted by the node as well and starts a new trajectory " + "using its settings.\n"); + google::ParseCommandLineFlags(&argc, &argv, true); + + CHECK(!FLAGS_configuration_directory.empty()) + << "-configuration_directory is missing."; + CHECK(!FLAGS_configuration_basename.empty()) + << "-configuration_basename is missing."; + + ::ros::init(argc, argv, "cartographer_start_trajectory"); + ::ros::start(); + + cartographer_ros::ScopedRosLogSink ros_log_sink; + int exit_code = cartographer_ros::Run() ? 0 : 1; + ::ros::shutdown(); + return exit_code; +} diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index d092217..5b0e943 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -50,4 +50,39 @@ TrajectoryOptions CreateTrajectoryOptions( return options; } + +bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, + TrajectoryOptions* options) { + options->tracking_frame = msg.tracking_frame; + options->published_frame = msg.published_frame; + options->odom_frame = msg.odom_frame; + options->provide_odom_frame = msg.provide_odom_frame; + options->use_odometry = msg.use_odometry; + options->use_laser_scan = msg.use_laser_scan; + options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan; + options->num_point_clouds = msg.num_point_clouds; + if (!options->trajectory_builder_options.ParseFromString( + msg.trajectory_builder_options_proto)) { + LOG(ERROR) << "Failed to parse protobuf"; + return false; + } + return true; +} + +cartographer_ros_msgs::TrajectoryOptions ToRosMessage( + const TrajectoryOptions& options) { + cartographer_ros_msgs::TrajectoryOptions msg; + msg.tracking_frame = options.tracking_frame; + msg.published_frame = options.published_frame; + msg.odom_frame = options.odom_frame; + msg.provide_odom_frame = options.provide_odom_frame; + msg.use_odometry = options.use_odometry; + msg.use_laser_scan = options.use_laser_scan; + msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan; + msg.num_point_clouds = options.num_point_clouds; + options.trajectory_builder_options.SerializeToString( + &msg.trajectory_builder_options_proto); + return msg; +} + } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index fd055e1..a02a130 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -22,6 +22,7 @@ #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" #include "cartographer_ros/sensor_bridge.h" +#include "cartographer_ros_msgs/TrajectoryOptions.h" namespace cartographer_ros { @@ -41,6 +42,14 @@ struct TrajectoryOptions { TrajectoryOptions CreateTrajectoryOptions( ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); +// Try to convert 'msg' into 'options'. Returns false on failure. +bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, + TrajectoryOptions* options); + +// Converts 'trajectory_options' into a ROS message. +cartographer_ros_msgs::TrajectoryOptions ToRosMessage( + const TrajectoryOptions& trajectory_options); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_