delete obsolete cartographer_ros, adding MPC node with two turtlesim, need to be fixed

MPC/CMakeLists.txt

@@ -0,0 +1,38 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(MPC)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+#add_compile_options(-std=c++11)
+set(CMAKE_CXX_STANDARD 11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+        nav_msgs
+        tf
+)
+
+catkin_package(
+
+)
+
+include_directories(
+        # include
+        "/usr/include/eigen3"
+        ${catkin_INCLUDE_DIRS}
+)
+link_directories(/usr/local/lib)
+add_executable(${PROJECT_NAME}_node src/node.cpp src/trajectory/make_trajectory.cpp
+        src/mpc/mpc_tools.cpp src/mpc/MonitorMPC.cpp src/trajectory/FourthTrajectory.cpp
+        src/mpc/TowBotMPC.cpp)
+target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ipopt)
+
+
+add_executable(${PROJECT_NAME}2_node src/node2.cpp src/trajectory/make_trajectory.cpp
+        src/mpc/mpc_tools.cpp src/mpc/MonitorMPC.cpp src/trajectory/FourthTrajectory.cpp
+        src/mpc/TowBotMPC.cpp)
+target_link_libraries(${PROJECT_NAME}2_node ${catkin_LIBRARIES} ipopt)

MPC/launch/MPC.launch

@@ -0,0 +1,8 @@
+<launch>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
+  <node pkg="MPC" type="MPC_node" name="MPC_node" output="screen"/>
+  <group ns="turtlesim1">
+     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+   </group>
+
+</launch>

MPC/launch/MPC_2.launch

@@ -0,0 +1,14 @@
+<launch>
+
+  <group ns="turtlesim1">
+     <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+   </group>
+
+  <group ns="turtlesim2">
+    <node pkg="turtlesim" name="sim" type="turtlesim_node"/>
+  </group>
+
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find MPC)/maps/blank_map.yaml"/>
+  <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/>
+
+</launch>

MPC/maps/blank_map.yaml

@@ -0,0 +1,6 @@
+image: blank_map.pgm
+resolution: 0.02
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

MPC/package.xml

@@ -0,0 +1,70 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>MPC</name>
+  <version>0.0.0</version>
+  <description>The MPC package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="zx@todo.todo">zx</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/MPC</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

MPC/src/mpc/MonitorMPC.cpp

@@ -0,0 +1,245 @@
+#include "MonitorMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+// Set the timestep length and duration
+// Currently tuned to predict 1 second worth
+size_t N = 10;
+
+
+// The solver takes all the state variables and actuator
+// variables in a singular vector. Thus, we should to establish
+// when one variable starts and another ends to make our lifes easier.
+size_t nx = 0;
+size_t ny = nx + N;
+size_t nth = ny + N;
+size_t nv = nth + N;
+size_t nvth = nv + N;
+
+
+class FG_eval {
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs;
+    double m_ref_v,m_dt;
+    double m_v,m_vth;
+    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
+        this->coeffs = coeffs;
+        m_v=v;
+        m_vth=vth;
+        m_ref_v=ref_v;
+        m_dt=dt;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+        // Implementing MPC below
+        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
+        // The cost is stored is the first element of `fg`.
+        // Any additions to the cost should be added to `fg[0]`.
+        fg[0] = 0;
+        double dt=m_dt;
+        // Reference State Cost
+        // Below defines the cost related the reference state and
+        // any anything you think may be beneficial.
+
+        // Weights for how "important" each cost is - can be tuned
+        const int y_cost_weight = 1500;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 20;
+        const int vth_cost_weight = 100;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight=100;
+
+        // Cost for CTE, psi error and velocity
+        for (int t = 0; t < N; t++) {
+            AD<double> xt=vars[nx+t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t]-fx, 2);
+
+            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
+
+            fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-m_ref_v,2);
+
+        }
+
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < N-1; t++) {
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
+            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t+1]-vars[nvth+t], 2);
+        }
+
+        /////////////////////
+        fg[1 + nx] = vars[nx]-vars[nv]*dt;
+        fg[1 + ny] = vars[ny];
+        fg[1 + nth] = vars[nth]-vars[nvth]*dt;
+
+        // The rest of the constraints
+        for (int t = 1; t < N; t++) {
+            // State at time t + 1
+            AD<double> x1 = vars[nx + t];
+            AD<double> y1 = vars[ny + t];
+            AD<double> th1 = vars[nth + t];
+            AD<double> v1 = vars[nv + t];
+            AD<double> vth1 = vars[nvth + t];
+
+            // State at time t
+            AD<double> x0 = vars[nx + t -1];
+            AD<double> y0 = vars[ny + t -1];
+            AD<double> th0 = vars[nth + t-1];
+            AD<double> v0 = vars[nv + t-1];
+            AD<double> vth0 = vars[nvth + t-1];
+
+            // Actuator constraints at time t only
+
+            // Setting up the rest of the model constraints
+            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
+            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
+            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);
+
+        }
+
+        fg[1 + nv]=(vars[nv]-m_v)/dt;
+        fg[1+nvth]=(vars[nvth]-m_vth)/dt;
+        for(int t=1;t<N;++t)
+        {
+            fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
+            fg[1+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
+        }
+
+        ////  add
+
+    }
+};
+
+//
+// MPC class definition implementation.
+//
+MonitorMPC::MonitorMPC() {}
+MonitorMPC::~MonitorMPC() {}
+
+bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+
+    // State vector holds all current values neede for vars below
+    double v = state[0];
+    double vth = state[1];
+    double ref_v = state[2];
+    double max_v=state[3];
+    double dt=state[4];
+
+
+    // Setting the number of model variables (includes both states and inputs).
+    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
+    size_t n_vars = N * 5;
+    // Setting the number of constraints
+    size_t n_constraints = N * 5;
+
+    // Initial value of the independent variables.
+    // SHOULD BE 0 besides initial state.
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    // Sets lower and upper limits for variables.
+    // Set all non-actuators upper and lowerlimits
+    // to the max negative and positive values.
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    //// limit  v
+    for (int i = nv; i < nv+N; i++) {
+        vars_lowerbound[i] = -max_v;
+        vars_upperbound[i] = max_v;
+    }
+
+    ////limint vth
+    for (int i = nvth; i < nvth+N; i++) {
+        vars_lowerbound[i] = -0.5;
+        vars_upperbound[i] = 0.5;
+    }
+
+    // Lower and upper limits for the constraints
+    // Should be 0 besides initial state.
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (int i = 0; i < n_constraints; i++) {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+    //// acc v
+    for (int i = nv; i < nv+N; i++) {
+        constraints_lowerbound[i] = -5.0;
+        constraints_upperbound[i] = 5.0;
+    }
+    //// acc vth
+    for (int i = nvth; i < nvth+N; i++) {
+        constraints_lowerbound[i] = -2.5;
+        constraints_upperbound[i] = 2.5;
+    }
+
+
+    // object that computes objective and constraints
+    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);
+
+    //
+    // NOTE: You don't have to worry about these options
+    //
+    // options for IPOPT solver
+    std::string options;
+    // Uncomment this if you'd like more print information
+    options += "Integer print_level  0\n";
+    // NOTE: Setting sparse to true allows the solver to take advantage
+    // of sparse routines, this makes the computation MUCH FASTER. If you
+    // can uncomment 1 of these and see if it makes a difference or not but
+    // if you uncomment both the computation time should go up in orders of
+    // magnitude.
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
+    // Change this as you see fit.
+    options += "Numeric max_cpu_time          0.5\n";
+
+    // place to return solution
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+    // solve the problem
+    CppAD::ipopt::solve<Dvector, FG_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    auto cost = solution.obj_value;
+    //std::cout << "Cost " << cost << std::endl;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory.clear();
+    out.push_back(solution.x[nv]);
+    out.push_back(solution.x[nvth]);
+    for (int i = 0; i < N; ++i) {
+        out.push_back(solution.x[nx + i]);
+        out.push_back(solution.x[ny + i]);
+        tf::Pose pose;
+        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
+        m_trajectory.push_back(pose);
+    }
+    printf(" cost : %.3f\n",cost);
+    return ok;
+
+}

MPC/src/mpc/MonitorMPC.h

@@ -0,0 +1,24 @@
+//
+// Created by zx on 2019/8/28.
+//
+
+#ifndef MONITORMPC_H
+#define MONITORMPC_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class MonitorMPC
+{
+public:
+    MonitorMPC();
+    virtual ~MonitorMPC();
+
+    bool Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory;
+};
+
+
+#endif //MONITORMPC_H

MPC/src/mpc/TowBotMPC.cpp

@@ -0,0 +1,327 @@
+//
+// Created by zx on 2019/9/3.
+//
+
+#include "TowBotMPC.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+#include "Eigen/QR"
+
+using CppAD::AD;
+
+size_t K=10;
+size_t nx1 = 0;
+size_t ny1 = nx1 + K;
+size_t nth1 = ny1 + K;
+size_t nv1 = nth1 + K;
+size_t nvth1 = nv1 + K;
+
+
+size_t nx2 = nvth1 + K;
+size_t ny2 = nx2 + K;
+size_t nth2 = ny2 + K;
+size_t nv2 = nth2 + K;
+size_t nvth2 = nv2 + K;
+
+double Lx1=-1.3;
+double Ly1=0;
+double Lx2=1.3;
+double Ly2=0;
+
+class FG_eval_2 {
+public:
+    // Fitted polynomial coefficients
+    Eigen::VectorXd coeffs,coeffs_b;
+    Eigen::VectorXd m_data;
+    FG_eval_2(Eigen::VectorXd coeffs,Eigen::VectorXd coeffs_b,Eigen::VectorXd P) {
+        this->coeffs = coeffs;
+        this->coeffs_b=coeffs_b;
+        m_data=P;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+
+        double v1=m_data[0];
+        double v2=m_data[1];
+        double vth1=m_data[2];
+        double vth2=m_data[3];
+        double ref_v=m_data[4];
+        double dt=m_data[5];
+
+        double dx=m_data[6];  /// 两车初始位姿差   前车相对后车坐标系
+        double dy=m_data[7];
+        double dth=m_data[8];   ///两坐标系角度
+
+
+
+        ///前车 cost weight
+        const int y_cost_weight = 1500;
+        const int th_cost_weight = 1000;
+        const int v_cost_weight = 20;
+
+        const int vth_cost_weight = 100;
+        const int a_cost_weight = 1;
+        const int ath_cost_weight=100;
+
+        ///将坐标旋转到后车坐标系下
+        double new_xt=cos(dth)*Lx1-sin(dth)*Ly1;
+        double new_yt=sin(dth)*Lx1+cos(dth)*Ly1;
+
+        double distance_x=new_xt-Lx2+dx;
+        double distance_y=new_yt-Ly2+dy;
+        printf(" Key Point distance xy: %.3f  %.3f\n",fabs(distance_x),fabs(distance_y));
+
+        fg[0] = 0;
+        ///后车 cost weight
+        const int back_y_cost_weight = 500;
+        const int back_th_cost_weight = 300;
+        const int back_v_cost_weight = 1;
+
+        const int back_vth_cost_weight = 3;
+        const int back_a_cost_weight = 1;
+        const int back_ath_cost_weight=5;
+
+        for (int t = 0; t < K; t++) {
+            AD<double> xt=vars[nx1+t];
+            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t]-fx, 2);
+
+            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t]-fth, 2);
+            fg[0]+=v_cost_weight*CppAD::pow(vars[nv1+t]-ref_v,2);
+
+            ////   loss back
+            AD<double> xt2=vars[nx2+t];
+            AD<double> fx2 = coeffs[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
+            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t]-fx2, 2);
+
+            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2*coeffs_b[2]*xt2 + 3*coeffs_b[3]*pow(xt2,2));
+            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t]-fth2, 2);
+            fg[0] +=back_v_cost_weight*CppAD::pow(vars[nv2+t]-ref_v,2);
+
+        }
+        //加速度 cost
+        // Costs for steering (delta) and acceleration (a)
+        for (int t = 0; t < K-1; t++) {
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
+            fg[0] += a_cost_weight * CppAD::pow(vars[nv1 + t+1]-vars[nv1+t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth1 + t+1]-vars[nvth1+t], 2);
+
+            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
+            fg[0] += back_a_cost_weight * CppAD::pow(vars[nv2 + t+1]-vars[nv2+t], 2);
+            fg[0] += back_ath_cost_weight * CppAD::pow(vars[nvth2 + t+1]-vars[nvth2+t], 2);
+        }
+        ///添加两车距离损失
+        const int distance_weight_x=10;
+        const int distance_weight_y=50;
+        const int delta_th_weight=1;
+        for(int t=0;t<K;++t)
+        {
+            AD<double> xt=vars[nx1+t]+Lx1;
+            AD<double> yt=vars[ny1+t]+Ly1;
+            ///将坐标旋转到后车坐标系下
+            AD<double> new_xt=CppAD::cos(dth)*xt-CppAD::sin(dth)*yt;
+            AD<double> new_yt=CppAD::sin(dth)*xt+CppAD::cos(dth)*yt;
+
+            AD<double> distance_x=new_xt-(vars[nx2+t]+Lx2)+dx;
+            AD<double> distance_y=new_yt-(vars[ny2+t]+Ly2)+dy;
+
+            fg[0]+=distance_weight_x*CppAD::pow(distance_x,2);//+distance_weight_y*CppAD::pow(distance_y,2);
+            fg[0]+=delta_th_weight*CppAD::pow(vars[nth1+t]-vars[nth2+t]+dth,2);
+
+        }
+
+        /////////////////////
+        fg[1 + nx1] = vars[nx1]-vars[nv1]*dt;
+        fg[1 + ny1] = vars[ny1];
+        fg[1 + nth1] = vars[nth1]-vars[nvth1]*dt;
+
+        fg[1 + nx2] = vars[nx2]-vars[nv2]*dt;
+        fg[1 + ny2] = vars[ny2];
+        fg[1 + nth2] = vars[nth2]-vars[nvth2]*dt;
+
+        // The rest of the constraints
+        for (int t = 1; t < K; t++) {
+            /// 位置约束 1
+            AD<double> x1 = vars[nx1 + t];
+            AD<double> y1 = vars[ny1 + t];
+            AD<double> th1 = vars[nth1 + t];
+            AD<double> v1 = vars[nv1 + t];
+            AD<double> vth1 = vars[nvth1 + t];
+
+            AD<double> x0 = vars[nx1 + t -1];
+            AD<double> y0 = vars[ny1 + t -1];
+            AD<double> th0 = vars[nth1 + t-1];
+            AD<double> v0 = vars[nv1 + t-1];
+            AD<double> vth0 = vars[nvth1 + t-1];
+
+            fg[1 + nx1 + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
+            fg[1 + ny1 + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
+            fg[1 + nth1 + t] = th1 - (th0 + vth0 * dt);
+
+            //位置约束2
+            AD<double> bx1 = vars[nx2 + t];
+            AD<double> by1 = vars[ny2 + t];
+            AD<double> bth1 = vars[nth2 + t];
+            AD<double> bv1 = vars[nv2 + t];
+            AD<double> bvth1 = vars[nvth2 + t];
+
+            AD<double> bx0 = vars[nx2 + t -1];
+            AD<double> by0 = vars[ny2 + t -1];
+            AD<double> bth0 = vars[nth2 + t-1];
+            AD<double> bv0 = vars[nv2 + t-1];
+            AD<double> bvth0 = vars[nvth2 + t-1];
+
+            fg[1 + nx2 + t] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
+            fg[1 + ny2 + t] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
+            fg[1 + nth2 + t] = bth1 - (bth0 + bvth0 * dt);
+
+        }
+
+        fg[1 + nv1]=(vars[nv1]-v1)/dt;
+        fg[1+nvth1]=(vars[nvth1]-vth1)/dt;
+
+        fg[1 + nv2]=(vars[nv2]-v2)/dt;
+        fg[1+nvth2]=(vars[nvth2]-vth2)/dt;
+        for(int t=1;t<K;++t)
+        {
+            fg[1+nv1+t]=(vars[nv1+t]-vars[nv1+t-1])/dt;
+            fg[1+nvth1+t]=(vars[nvth1+t]-vars[nvth1+t-1])/dt;
+            fg[1+nv2+t]=(vars[nv2+t]-vars[nv2+t-1])/dt;
+            fg[1+nvth2+t]=(vars[nvth2+t]-vars[nvth2+t-1])/dt;
+        }
+
+    }
+};
+
+bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b,std::vector<double> &out)
+{
+    bool ok = true;
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+    // State vector holds all current values neede for vars below
+
+    double v1=state[0];
+    double v2=state[1];
+    double vth1=state[2];
+    double vth2=state[3];
+    double ref_v=state[4];
+    double dt=state[5];
+
+    double dx=state[6];  /// 两车初始位姿差   前车相对后车坐标系
+    double dy=state[7];
+    double dth=state[8];   ///两坐标系角度
+
+    size_t n_vars = K * 5 *2;
+    size_t n_constraints = K * 5 *2;
+
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+    for(int i=0;i<K;++i)
+    {
+        vars[nv1+i]=v1;
+        vars[nv2+i]=v2;
+
+        vars[nvth1+i]=vth1;
+        vars[nvth2+i]=vth2;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    for (int i = 0; i < K; i++) {
+        //// limit  v1 v2
+        vars_lowerbound[nv1+i] = -max_v;
+        vars_upperbound[nv1+i] = max_v;
+
+        vars_lowerbound[nv2+i] = -max_v;
+        vars_upperbound[nv2+i] = max_v;
+
+        //// limit  vth1 2
+        vars_lowerbound[nvth1+i] = -0.5;
+        vars_upperbound[nvth1+i] = 0.5;
+
+        vars_lowerbound[nvth2+i] = -0.5;
+        vars_upperbound[nvth2+i] = 0.5;
+    }
+    ////   constraints
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+    for (int i = 0; i < n_constraints; i++) {
+        constraints_lowerbound[i] = 0;
+        constraints_upperbound[i] = 0;
+    }
+
+    for (int i = 0; i < K; i++) {
+        ////acc v1 v2
+        constraints_lowerbound[nv1+i] = -5.0;
+        constraints_upperbound[nv1+i] = 5.0;
+        constraints_lowerbound[nv2+i] = -5.0;
+        constraints_upperbound[nv2+i] = 5.0;
+
+        //acc th 1 2
+        constraints_lowerbound[nvth1+i] = -2.5;
+        constraints_upperbound[nvth1+i] = 2.5;
+        constraints_lowerbound[nvth2+i] = -2.5;
+        constraints_upperbound[nvth2+i] = 2.5;
+    }
+
+    // object that computes objective and constraintsv
+    FG_eval_2 fg_eval(coeffs,coeffs_b,state);
+
+
+    std::string options;
+    options += "Integer print_level  0\n";
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    options += "Numeric max_cpu_time          0.5\n";
+
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+
+    CppAD::ipopt::solve<Dvector, FG_eval_2>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    auto cost = solution.obj_value;
+
+    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
+    out.clear();
+    m_trajectory_f.clear();
+    m_trajectory_b.clear();
+    out.push_back(solution.x[nv1]);
+    out.push_back(solution.x[nvth1]);
+    out.push_back(solution.x[nv2]);
+    out.push_back(solution.x[nvth2]);
+    for (int i = 0; i < K; ++i) {
+        tf::Pose pose1,pose2;
+        pose1.setOrigin(tf::Vector3(solution.x[nx1+i],solution.x[ny1+i],0));
+        pose2.setOrigin(tf::Vector3(solution.x[nx2+i],solution.x[ny2+i],0));
+        m_trajectory_f.push_back(pose1);
+        m_trajectory_b.push_back(pose2);
+    }
+
+    /*printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
+        ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);*/
+
+    return ok;
+
+}
+
+TowBotMPC::TowBotMPC()
+{}
+TowBotMPC::~TowBotMPC()
+{}
+

MPC/src/mpc/TowBotMPC.h

@@ -0,0 +1,25 @@
+//
+// Created by zx on 2019/9/3.
+//
+
+#ifndef TOWBOTMPC_H
+#define TOWBOTMPC_H
+#include <vector>
+#include <Eigen/Core>
+#include <tf/transform_datatypes.h>
+
+class TowBotMPC
+{
+public:
+    TowBotMPC();
+    ~TowBotMPC();
+    bool Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs,
+        Eigen::VectorXd coeffs_b, std::vector<double>& out);
+
+    std::vector<tf::Pose> m_trajectory_f;
+    std::vector<tf::Pose> m_trajectory_b;
+
+};
+
+
+#endif //TOWBOTMPC_H

MPC/src/mpc/mpc_tools.cpp

@@ -0,0 +1,78 @@
+#include  "mpc_tools.h"
+
+constexpr double pi() { return M_PI; }
+double deg2rad(double x) { return x * pi() / 180; }
+double rad2deg(double x) { return x * 180 / pi(); }
+
+
+mpc_tools::mpc_tools()
+{
+}
+
+mpc_tools::~mpc_tools()
+{
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_tools::filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
+    int numOfPoints,bool direct){
+  if(poses.size() < numOfPoints)
+    return false;
+
+ out.clear();
+  // 平移加反向旋转到小车坐标系
+    double theta,a,b;
+    pose.getBasis().getEulerZYX(theta,a,b);
+    if(direct==true) theta+=M_PI;
+  for (int i = 0; i < poses.size(); i++)
+  {
+
+    double x = poses[i].getOrigin().getX()-pose.getOrigin().getX();
+    double y = poses[i].getOrigin().getY()-pose.getOrigin().getY();
+    double trans_x = x * cos(-theta) - y * sin(-theta);
+    double trans_y = x * sin(-theta) + y * cos(-theta);
+    if (trans_x>=0  && out.size() < numOfPoints)
+    {
+        tf::Pose point;
+        float nx=trans_x * cos(theta) - trans_y * sin(theta);
+        float ny=trans_x*sin(theta)+trans_y*cos(theta);
+        point.setOrigin(tf::Vector3(nx+pose.getOrigin().getX(),
+            ny+pose.getOrigin().getY(),0));
+        out.push_back(point);
+
+    }
+  }
+  return true;
+
+}
+
+// 求y
+double mpc_tools::polyeval(Eigen::VectorXd coeffs, double x) {
+  double result = 0.0;
+  for (int i = 0; i < coeffs.size(); i++) {
+    result += coeffs[i] * pow(x, i);
+  }
+  return result;
+}
+
+// 求参数
+Eigen::VectorXd mpc_tools::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
+  assert(xvals.size() == yvals.size());
+  assert(order >= 1 && order <= xvals.size() - 1);
+  Eigen::MatrixXd A(xvals.size(), order + 1);
+
+  for (int i = 0; i < xvals.size(); i++) {
+    A(i, 0) = 1.0;
+  }
+
+  for (int j = 0; j < xvals.size(); j++) {
+    for (int i = 0; i < order; i++) {
+      A(j, i + 1) = A(j, i) * xvals(j);
+    }
+  }
+
+  auto Q = A.householderQr();
+  auto result = Q.solve(yvals);
+  return result;
+}
+

MPC/src/mpc/mpc_tools.h

@@ -0,0 +1,29 @@
+#ifndef MPC_TOOLS_HH
+#define MPC_TOOLS_HH 
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <tf/transform_datatypes.h>
+
+
+// For converting back and forth between radians and degrees.
+
+
+class mpc_tools{
+
+public:
+    mpc_tools();
+    ~mpc_tools();
+    //求参
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+    //求y
+    double polyeval(Eigen::VectorXd coeffs, double x);
+
+    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out,
+        int numOfPoints,bool derect=false);
+
+
+};
+
+#endif /* MPC_TOOLS_HH */

MPC/src/node.cpp

@@ -0,0 +1,291 @@
+//
+// Created by zx on 2019/8/26.
+//
+
+#include <iostream>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Path.h>
+#include <string>
+#include <chrono>
+
+#include "mpc/MonitorMPC.h"
+#include "trajectory/make_trajectory.h"
+#include "trajectory/FourthTrajectory.h"
+#include "mpc/mpc_tools.h"
+
+ros::Publisher g_vel_pub;
+ros::Publisher g_local_plan_pub,g_global_plan_pub,g_fit_plan_pub;
+tf::Pose g_cur_pose,g_goal;
+bool g_goal_valid=false;
+
+FourthTrajectory maker;
+mpc_tools mpc_tool;
+double g_max_vx;
+
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("new goal recieve ...  \n");
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+    g_goal.setOrigin(tf::Vector3(x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+
+    if(maker.Make(g_cur_pose,g_goal,20)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+
+    }
+
+}
+
+void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+{
+    ROS_INFO_ONCE("odom received!");
+    static tf::TransformListener tf_listen(ros::Duration(10));
+    static double dt=0.05;
+    auto start = std::chrono::system_clock::now();
+
+    double vx = msg->twist.twist.linear.x;
+    double vth = msg->twist.twist.angular.z;
+    std::string frame_id = msg->child_frame_id;
+    std::string tf_error;
+    tf::StampedTransform transform;
+    try{
+        tf_listen.lookupTransform("/map", "/base_link",ros::Time(0), transform);
+        g_cur_pose=transform;
+    }
+    catch (tf::TransformException &ex) {
+        if(g_goal_valid)
+            ROS_ERROR(" tf listen error : %s",ex.what());
+        return;
+    }
+    int fitNum=20;
+    if(g_goal_valid==false || maker.Size()<fitNum ) return ;
+//////判断是否到达目标点
+    double x = transform.getOrigin().getX();
+    double y = transform.getOrigin().getY();
+    double gx = g_goal.getOrigin().getX();
+    double gy = g_goal.getOrigin().getY();
+    double yaw;
+    double theta,a,b;
+    transform.getBasis().getEulerZYX(theta,a,b);
+    g_goal.getBasis().getEulerZYX(yaw, a, b);
+
+    double dx=g_goal.getOrigin().getX()-transform.getOrigin().getX();
+    double dy=g_goal.getOrigin().getY()-transform.getOrigin().getY();
+    double dis_th = fabs(yaw - theta);
+    if (fabs(dx) < 0.05 && fabs(dy) < 0.03 && dis_th < 0.02 && fabs(vx) < 0.05 && fabs(vth) < 0.05)
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = 0;
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = 0;
+        g_vel_pub.publish(cmd_vel);
+
+        g_goal_valid = false;
+        std::cout<<"   到达目标点 !!!!"<<std::endl;
+        return;
+    }
+
+    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
+    tf::Vector3 length(dx,dy,0);
+
+    double direct_x = dx * cos(-theta) - dy * sin(-theta);
+
+    double ref_v=length.length()/(g_max_vx*1.5);
+
+    ref_v=std::max(ref_v,0.02);
+    ref_v=std::min(ref_v,g_max_vx);
+
+    if (direct_x<0) ref_v=-ref_v;
+
+    ///// 选点
+    std::vector<tf::Pose> poses;
+    if(!mpc_tool.filterPath(transform,maker.trajectory_,poses,fitNum,direct_x<0))
+    {
+        ROS_ERROR(" fit error ...");
+        return ;
+    }
+    else
+    {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for(unsigned int i=0; i < poses.size(); i++)
+        {
+            float x=poses[i].getOrigin().getX();
+            float y=poses[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x=x;
+            gui_path.poses[i].pose.position.y=y;
+            tf::Quaternion quat=poses[i].getRotation();
+            gui_path.poses[i].pose.orientation.x=quat.x();
+            gui_path.poses[i].pose.orientation.y=quat.y();
+            gui_path.poses[i].pose.orientation.z=quat.z();
+            gui_path.poses[i].pose.orientation.w=quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub.publish(gui_path);
+
+    }
+
+    //////   fit   select points
+    Eigen::VectorXd ptsx_car(poses.size());
+    Eigen::VectorXd ptsy_car(poses.size());
+
+    for (int i = 0; i < poses.size(); i++)
+    {
+        double x = poses[i].getOrigin().getX() - transform.getOrigin().getX();
+        double y = poses[i].getOrigin().getY() - transform.getOrigin().getY();
+        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
+        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
+    }
+
+    Eigen::VectorXd coeffs=mpc_tool.polyfit(ptsx_car,ptsy_car,3);
+
+    Eigen::VectorXd state(5);
+    state << vx, vth, ref_v, g_max_vx,dt;
+    MonitorMPC mpc;
+    std::vector<double> out;
+
+    if(mpc.Solve(state,coeffs,out))
+    {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = out[0];
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = out[1];
+        g_vel_pub.publish(cmd_vel);
+
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(mpc.m_trajectory.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory.size(); i++) {
+            float x = mpc.m_trajectory[i].getOrigin().getX();
+            float y = mpc.m_trajectory[i].getOrigin().getY();
+            float rx = x * cos(theta) - y * sin(theta) + transform.getOrigin().getX();
+            float ry = x * sin(theta) + y * cos(theta) + transform.getOrigin().getY();
+            gui_path.poses[i].pose.position.x = rx;
+            gui_path.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub.publish(gui_path);
+
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+        printf(" ref:%.3f, best_v:%.3f, best_th:%.3f, dt:%.3f\n",ref_v,out[0],out[1],time);
+        double sleep_t=(dt-time)*1000.0;
+        if(sleep_t>0)
+            usleep(1000*int(sleep_t));
+
+
+    }
+
+
+}
+void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("update goal recieve ...  \n");
+    double x=g_cur_pose.getOrigin().getX();
+    double y=g_cur_pose.getOrigin().getY();
+    double z=g_cur_pose.getOrigin().getZ();
+    double qx=g_cur_pose.getRotation().getX();
+    double qy=g_cur_pose.getRotation().getY();
+    double qz=g_cur_pose.getRotation().getZ();
+    double qw=g_cur_pose.getRotation().getW();
+    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+    if(maker.Make(g_cur_pose,g_goal,10)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+    }
+}
+int main(int argc ,char* argv[])
+{
+    std::cout<<"  input max vx : ";
+    std::cin>>g_max_vx;
+    std::cout<<std::endl;
+
+    ros::init(argc, argv, "MPC_node");
+
+    ros::NodeHandle gn;
+
+    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);
+
+    ros::Subscriber odom_sub_ = gn.subscribe<nav_msgs::Odometry>( "/odom", 1, odomCallback);
+
+    g_vel_pub = gn.advertise<geometry_msgs::Twist>("cmd_vel", 1);
+    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
+    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
+    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
+
+    ros::spin();
+
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0.0;
+    cmd_vel.angular.z = 0;
+    g_vel_pub.publish(cmd_vel);
+
+    return 0;
+}

MPC/src/node2.cpp

@@ -0,0 +1,483 @@
+//
+// Created by zx on 2019/9/6.
+//
+
+//
+// Created by zx on 2019/8/26.
+//
+
+#include <iostream>
+#include <tf/transform_datatypes.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Path.h>
+#include <string>
+#include <chrono>
+#include <thread>
+#include <turtlesim/Pose.h>
+
+#include "mpc/TowBotMPC.h"
+#include "trajectory/make_trajectory.h"
+#include "trajectory/FourthTrajectory.h"
+#include "mpc/mpc_tools.h"
+#include <tf/transform_broadcaster.h>
+
+ros::Publisher g_vel_pub1,g_vel_pub2;
+ros::Publisher g_pose_pub1,g_pose_pub2;
+ros::Publisher g_local_plan_pub,g_local_plan_pub_b,g_global_plan_pub,g_fit_plan_pub,g_fit_plan_pub_b;
+tf::Pose g_pose_f,g_pose_b,g_goal;
+geometry_msgs::Twist g_cmd_vel_f,g_cmd_vel_b;
+bool g_goal_valid=false;
+int g_direction=1;      //朝向  向前为0  先后为1
+
+FourthTrajectory maker;
+mpc_tools mpc_tool;
+double g_max_vx;
+
+void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("new goal recieve ...  \n");
+    double x=msg->pose.position.x;
+    double y=msg->pose.position.y;
+    double z=msg->pose.position.z;
+    double qx=msg->pose.orientation.x;
+    double qy=msg->pose.orientation.y;
+    double qz=msg->pose.orientation.z;
+    double qw=msg->pose.orientation.w;
+    g_goal.setOrigin(tf::Vector3(x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    double distance1 = g_pose_f.getOrigin().distance(g_goal.getOrigin());
+    double distance2 = g_pose_b.getOrigin().distance(g_goal.getOrigin());
+    tf::Pose start_point=distance1>distance2?g_pose_f:g_pose_b;
+    g_direction=distance1>distance2?-1:1;
+    ///更新轨迹
+
+    if(maker.Make(start_point,g_goal,50)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+
+    }
+
+}
+
+void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
+{
+    double theta=msg->theta;
+    tf::Quaternion q;
+    q.setRPY(0,0,theta);
+    g_pose_f.setRotation(q);
+    g_pose_f.setOrigin(tf::Vector3(msg->x-2.0,msg->y,0));
+
+    g_cmd_vel_f.linear.x=msg->linear_velocity;
+    g_cmd_vel_f.angular.z=msg->angular_velocity;
+
+    geometry_msgs::PoseStamped pose;
+    pose.header.frame_id="map";
+    pose.header.stamp=ros::Time::now();
+    pose.pose.position.x=g_pose_f.getOrigin().getX();
+    pose.pose.position.y=g_pose_f.getOrigin().getY();
+    pose.pose.position.z=0;
+    pose.pose.orientation.x=q.x();
+    pose.pose.orientation.y=q.y();
+    pose.pose.orientation.z=q.z();
+    pose.pose.orientation.w=q.w();
+    g_pose_pub1.publish(pose);
+
+    static tf::TransformBroadcaster broadcaster;
+    tf::Pose conn_point;
+    conn_point.setOrigin(tf::Vector3(-1.3,0,0));
+    conn_point.setRotation(tf::Quaternion(0,0,0,1));
+    tf::Pose conn=g_pose_f*conn_point;
+
+    broadcaster.sendTransform(
+        tf::StampedTransform(conn,ros::Time::now(),"map", "conn1"));
+
+
+}
+
+void poseCallback2(const turtlesim::Pose::ConstPtr& msg)
+{
+    double theta=msg->theta;
+    tf::Quaternion q;
+    q.setRPY(0,0,theta);
+
+    g_pose_b.setRotation(q);
+    g_pose_b.setOrigin(tf::Vector3(msg->x-5.0,msg->y,0));
+
+    g_cmd_vel_b.linear.x=msg->linear_velocity;
+    g_cmd_vel_b.angular.z=msg->angular_velocity;
+
+    geometry_msgs::PoseStamped pose;
+    pose.header.frame_id="map";
+    pose.header.stamp=ros::Time::now();
+    pose.pose.position.x=g_pose_b.getOrigin().getX();
+    pose.pose.position.y=g_pose_b.getOrigin().getY();
+    pose.pose.position.z=0;
+    pose.pose.orientation.x=q.x();
+    pose.pose.orientation.y=q.y();
+    pose.pose.orientation.z=q.z();
+    pose.pose.orientation.w=q.w();
+    g_pose_pub2.publish(pose);
+
+    static tf::TransformBroadcaster broadcaster;
+    tf::Pose conn_point;
+    conn_point.setOrigin(tf::Vector3(1.3,0,0));
+    conn_point.setRotation(tf::Quaternion(0,0,0,1));
+    tf::Pose conn=g_pose_b*conn_point;
+
+    broadcaster.sendTransform(
+        tf::StampedTransform(conn,ros::Time::now(),"map", "conn2"));
+}
+
+void publish_cmd(geometry_msgs::Twist cmd_vel1,geometry_msgs::Twist cmd_vel2)
+{
+    ////
+    // g_vel_pub1.publish(cmd_vel1);
+    // g_vel_pub2.publish(cmd_vel2);
+
+}
+
+bool MPC()
+{
+    static tf::TransformListener tf_listen(ros::Duration(10));
+    static double dt = 0.05;
+    auto start = std::chrono::system_clock::now();
+
+    std::string tf_error;
+
+    int fitNum = 10;
+    if (g_goal_valid == false || maker.Size() < fitNum) return false;
+
+//////判断是否到达目标点
+    double yaw;
+    double theta, a, b;
+    g_pose_f.getBasis().getEulerZYX(theta, a, b);
+    g_goal.getBasis().getEulerZYX(yaw, a, b);
+
+    double theta_b;
+    g_pose_b.getBasis().getEulerZYX(theta_b, a, b);
+
+    double dis_th=0,offsetx = 0, offsety = 0,liner_vx=0,angular_vz=0;
+    if (g_direction == 1) {
+        dis_th=fabs(yaw - theta);
+        offsetx = g_goal.getOrigin().getX() - g_pose_f.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_f.getOrigin().getY();
+        liner_vx=g_cmd_vel_f.linear.x;
+        angular_vz=g_cmd_vel_f.angular.z;
+    }else {
+        dis_th=fabs(yaw - theta_b);
+        offsetx = g_goal.getOrigin().getX() - g_pose_b.getOrigin().getX();
+        offsety = g_goal.getOrigin().getY() - g_pose_b.getOrigin().getY();
+        liner_vx=g_cmd_vel_b.linear.x;
+        angular_vz=g_cmd_vel_b.angular.z;
+    }
+    if (fabs(offsetx) < 0.05 && fabs(offsety) < 0.03 && dis_th < 0.02 && fabs(liner_vx) < 0.05
+        && fabs(angular_vz) < 0.05) {
+        geometry_msgs::Twist cmd_vel;
+        cmd_vel.linear.x = 0;
+        cmd_vel.linear.y = 0.0;
+        cmd_vel.angular.z = 0;
+        publish_cmd(cmd_vel, cmd_vel);
+
+        g_goal_valid = false;
+        std::cout << "   到达目标点 !!!!" << std::endl;
+        return true;
+    }
+
+
+
+    //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
+    double dx=offsetx,dy=offsety;
+
+    tf::Vector3 length(dx, dy, 0);
+    double direct_x = dx * cos(-theta) - dy * sin(-theta);
+    double ref_v = length.length() / (g_max_vx * 1.5);
+    ref_v = std::max(ref_v, 0.02);
+    ref_v = std::min(ref_v, g_max_vx);
+    if (direct_x < 0) ref_v = -ref_v;
+
+    ///// 选点
+    std::vector<tf::Pose> poses;
+    std::vector<tf::Pose> poses_b;
+    if (!mpc_tool.filterPath(g_pose_f, maker.trajectory_, poses, fitNum, direct_x < 0)) {
+        ROS_ERROR(" fit error ...");
+        return false;
+    }
+    else {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < poses.size(); i++) {
+            float x = poses[i].getOrigin().getX();
+            float y = poses[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x = x;
+            gui_path.poses[i].pose.position.y = y;
+            tf::Quaternion quat = poses[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub.publish(gui_path);
+
+    }
+    if (!mpc_tool.filterPath(g_pose_b, maker.trajectory_, poses_b, fitNum, direct_x < 0)) {
+        ROS_ERROR(" fit error ...");
+        return false;
+    }
+    else {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(poses.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < poses_b.size(); i++) {
+            float x = poses_b[i].getOrigin().getX();
+            float y = poses_b[i].getOrigin().getY();
+
+            gui_path.poses[i].pose.position.x = x;
+            gui_path.poses[i].pose.position.y = y;
+            tf::Quaternion quat = poses_b[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_fit_plan_pub_b.publish(gui_path);
+
+    }
+
+
+    //////   fit   select points
+    Eigen::VectorXd ptsx_car(poses.size());
+    Eigen::VectorXd ptsy_car(poses.size());
+
+    for (int i = 0; i < poses.size(); i++) {
+        double x = poses[i].getOrigin().getX() - g_pose_f.getOrigin().getX();
+        double y = poses[i].getOrigin().getY() - g_pose_f.getOrigin().getY();
+        ptsx_car[i] = x * cos(-theta) - y * sin(-theta);
+        ptsy_car[i] = x * sin(-theta) + y * cos(-theta);
+    }
+    Eigen::VectorXd coeffs = mpc_tool.polyfit(ptsx_car, ptsy_car, 3);
+
+    Eigen::VectorXd ptsx_car_b(poses_b.size());
+    Eigen::VectorXd ptsy_car_b(poses_b.size());
+    for (int i = 0; i < poses_b.size(); i++) {
+        double x = poses_b[i].getOrigin().getX() - g_pose_b.getOrigin().getX();
+        double y = poses_b[i].getOrigin().getY() - g_pose_b.getOrigin().getY();
+        ptsx_car_b[i] = x * cos(-theta_b) - y * sin(-theta_b);
+        ptsy_car_b[i] = x * sin(-theta_b) + y * cos(-theta_b);
+    }
+
+    Eigen::VectorXd coeffs_b = mpc_tool.polyfit(ptsx_car_b, ptsy_car_b, 3);
+
+    Eigen::VectorXd state(9);
+    double vx1 = g_cmd_vel_f.linear.x;
+    double vx2 = g_cmd_vel_b.linear.x;
+    double vth1 = g_cmd_vel_f.angular.z;
+    double vth2 = g_cmd_vel_b.angular.z;
+    double Dx = g_pose_f.getOrigin().getX() - g_pose_b.getOrigin().getX();
+    double Dy = g_pose_f.getOrigin().getY() - g_pose_b.getOrigin().getY();
+    double Dth = theta - theta_b;
+
+
+    /*printf(" (%.3f %.3f ) , (%.3f %.3f)  (%.3f %.3f | %.3f %.3f)\n",g_pose_f.getOrigin().getX(),
+        g_pose_f.getOrigin().getY(),g_pose_b.getOrigin().getX(),g_pose_b.getOrigin().getY(),
+        g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/
+
+    state << vx1, vx2, vth1, vth2, ref_v, dt, Dx, Dy, Dth;
+    TowBotMPC mpc;
+    std::vector<double> out;
+    if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {
+        geometry_msgs::Twist cmd_vel1, cmd_vel2;
+        cmd_vel1.linear.x = out[0];
+        cmd_vel1.linear.y = 0.0;
+        cmd_vel1.angular.z = out[1];
+
+        cmd_vel2.linear.x = out[2];
+        cmd_vel2.linear.y = 0.0;
+        cmd_vel2.angular.z = out[3];
+        publish_cmd(cmd_vel1, cmd_vel2);
+
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(mpc.m_trajectory_f.size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory_f.size(); i++) {
+            float x = mpc.m_trajectory_f[i].getOrigin().getX();
+            float y = mpc.m_trajectory_f[i].getOrigin().getY();
+            float rx = x * cos(theta) - y * sin(theta) + g_pose_f.getOrigin().getX();
+            float ry = x * sin(theta) + y * cos(theta) + g_pose_f.getOrigin().getY();
+            gui_path.poses[i].pose.position.x = rx;
+            gui_path.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory_f[i].getRotation();
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub.publish(gui_path);
+
+        nav_msgs::Path gui_path_b;
+        gui_path_b.poses.resize(mpc.m_trajectory_b.size());
+        gui_path_b.header.frame_id = "map";
+        gui_path_b.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < mpc.m_trajectory_b.size(); i++) {
+            float x = mpc.m_trajectory_b[i].getOrigin().getX();
+            float y = mpc.m_trajectory_b[i].getOrigin().getY();
+            float rx = x * cos(theta_b) - y * sin(theta_b) + g_pose_b.getOrigin().getX();
+            float ry = x * sin(theta_b) + y * cos(theta_b) + g_pose_b.getOrigin().getY();
+            gui_path_b.poses[i].pose.position.x = rx;
+            gui_path_b.poses[i].pose.position.y = ry;
+            tf::Quaternion quat = mpc.m_trajectory_b[i].getRotation();
+            gui_path_b.poses[i].pose.orientation.x = quat.x();
+            gui_path_b.poses[i].pose.orientation.y = quat.y();
+            gui_path_b.poses[i].pose.orientation.z = quat.z();
+            gui_path_b.poses[i].pose.orientation.w = quat.w();
+        }
+        //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
+        g_local_plan_pub_b.publish(gui_path_b);
+
+        auto end = std::chrono::system_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+        double time =
+            double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+        //printf(" ref:%.3f, best_v:%.3f, best_th:%.3f, dt:%.3f\n",ref_v,out[0],out[1],time);
+        double sleep_t = (dt - time) * 1000.0;
+        if (sleep_t > 0)
+            usleep(1000 * int(sleep_t));
+
+    }
+}
+
+void thread_MPC()
+{
+    ros::Rate loop_rate(20);
+    while(ros::ok())
+    {
+        if(g_goal_valid==true)
+        {
+            bool ret = MPC();
+            if (ret) {
+
+            }
+            else {
+                ROS_WARN(" MPC failed ... ");
+            }
+        }
+        loop_rate.sleep();
+    }
+
+}
+
+// 更新目标，生成路径
+void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+    printf("update goal recieve ...  \n");
+    double x=g_pose_f.getOrigin().getX();
+    double y=g_pose_f.getOrigin().getY();
+    double z=g_pose_f.getOrigin().getZ();
+    double qx=g_pose_f.getRotation().getX();
+    double qy=g_pose_f.getRotation().getY();
+    double qz=g_pose_f.getRotation().getZ();
+    double qw=g_pose_f.getRotation().getW();
+    g_goal.setOrigin(tf::Vector3(x+msg->pose.position.x,y,z));
+    tf::Quaternion quat(qx,qy,qz,qw);
+    g_goal.setRotation(quat);
+
+    ///更新轨迹
+    if(maker.Make(g_pose_f,g_goal,10)) {
+        nav_msgs::Path gui_path;
+        gui_path.poses.resize(maker.Size());
+        gui_path.header.frame_id = "map";
+        gui_path.header.stamp = ros::Time::now();
+
+        // Extract the plan in world co-ordinates, we assume the path is all in the same frame
+        for (unsigned int i = 0; i < maker.Size(); i++) {
+            gui_path.poses[i].pose.position.x = maker[i].getOrigin().getX();
+            gui_path.poses[i].pose.position.y = maker[i].getOrigin().getY();
+            tf::Quaternion quat=maker[i].getRotation();
+
+            gui_path.poses[i].pose.orientation.x = quat.x();
+            gui_path.poses[i].pose.orientation.y = quat.y();
+            gui_path.poses[i].pose.orientation.z = quat.z();
+            gui_path.poses[i].pose.orientation.w = quat.w();
+        }
+
+        g_global_plan_pub.publish(gui_path);
+        g_goal_valid=true;
+    }
+}
+
+int main(int argc ,char* argv[])
+{
+    std::cout<<"  input max vx : ";
+    std::cin>>g_max_vx;
+    std::cout<<std::endl;
+
+    ros::init(argc, argv, "MPC_node");
+
+    ros::NodeHandle gn;
+
+    ros::Subscriber sub = gn.subscribe("/move_base_simple/goal",1,goalCallback);//move_base_simple
+    ros::Subscriber sub0 = gn.subscribe("/goal",1,updateGoal);
+
+    ros::Subscriber pose_sub1_ = gn.subscribe<turtlesim::Pose>( "/turtlesim1/turtle1/pose", 1, poseCallback1);
+    ros::Subscriber pose_sub2_ = gn.subscribe<turtlesim::Pose>( "/turtlesim2/turtle1/pose", 1, poseCallback2);
+
+    g_pose_pub1=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose1", 1);
+    g_pose_pub2=gn.advertise<geometry_msgs::PoseStamped>("/turtlePose2", 1);
+
+    g_vel_pub1 = gn.advertise<geometry_msgs::Twist>("/turtlesim1/turtle1/cmd_vel", 1);
+    g_vel_pub2 = gn.advertise<geometry_msgs::Twist>("/turtlesim2/turtle1/cmd_vel", 1);
+
+    g_global_plan_pub = gn.advertise<nav_msgs::Path>("global_plan",1, true);
+    g_local_plan_pub = gn.advertise<nav_msgs::Path>("local_plan", 1);
+    g_local_plan_pub_b = gn.advertise<nav_msgs::Path>("local_plan_b", 1);
+    g_fit_plan_pub = gn.advertise<nav_msgs::Path>("fit_plan", 1);
+    g_fit_plan_pub_b = gn.advertise<nav_msgs::Path>("fit_plan_b", 1);
+
+    std::thread* pthread=new std::thread(thread_MPC);
+    pthread->detach();
+
+    ros::spin();
+
+
+    geometry_msgs::Twist cmd_vel;
+    cmd_vel.linear.x = 0;
+    cmd_vel.linear.y = 0.0;
+    cmd_vel.angular.z = 0;
+    publish_cmd(cmd_vel,cmd_vel);
+
+    return 0;
+}

MPC/src/trajectory/CppAD5Trajectory.cpp

@@ -0,0 +1,244 @@
+//
+// Created by zx on 2019/8/30.
+//
+
+#include "CppAD5Trajectory.h"
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include "Eigen/Core"
+
+
+using CppAD::AD;
+
+class FG5_eval {
+public:
+    double m_x0,m_x1;
+    FG5_eval(double x0,double x1) {
+        m_x0=x0;
+        m_x1=x1;
+    }
+
+    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+    void operator()(ADvector& fg, const ADvector& vars) {
+        AD<double> a=vars[0];
+        AD<double> b=vars[1];
+        AD<double> c=vars[2];
+        AD<double> d=vars[3];
+        AD<double> e=vars[4];
+        AD<double> f=vars[5];
+
+        /*AD<double> Fx1=20*a*a*CppAD::pow(m_x1,5)+30*a*b*CppAD::pow(m_x1,4)+(20*a*c+b*b)*CppAD::pow(m_x1,3)
+            +(18*b*c+10*a*d)*CppAD::pow(m_x1,2)+(9*c*c+12*b*d)*m_x1+d*log(m_x1);
+        AD<double> Fx0=20*a*a*CppAD::pow(m_x0,5)+30*a*b*CppAD::pow(m_x0,4)+(20*a*c+b*b)*CppAD::pow(m_x0,3)
+            +(18*b*c+10*a*d)*CppAD::pow(m_x0,2)+(9*c*c+12*b*d)*m_x0+d*CppAD::log(m_x0);*/
+        AD<double> Fx1=100/7.0*a*a*CppAD::pow(m_x1,7)+20*a*b*CppAD::pow(m_x1,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x1,5)
+            +(9*b*c+5*a*d)*CppAD::pow(m_x1,4)+(3*c*c+4*b*d)*CppAD::pow(m_x1,3)+3*c*d*m_x1*m_x1+d*d;
+        AD<double> Fx0=100/7.0*a*a*CppAD::pow(m_x0,7)+20*a*b*CppAD::pow(m_x0,6)+(12*a*c+36/5.0*b*b)*CppAD::pow(m_x0,5)
+            +(9*b*c+5*a*d)*CppAD::pow(m_x0,4)+(3*c*c+4*b*d)*CppAD::pow(m_x0,3)+3*c*d*m_x0*m_x0+d*d;
+        fg[0]=Fx1-Fx0;      //||二阶导数|| * ||x-x1|| 在(xo,x1)上积分
+
+        fg[1]=a*CppAD::pow(m_x0,5)+b*CppAD::pow(m_x0,4)+c*CppAD::pow(m_x0,3)+d*CppAD::pow(m_x0,2)+e*m_x0+f; // ==y0
+        fg[2]=a*CppAD::pow(m_x1,5)+b*CppAD::pow(m_x1,4)+c*CppAD::pow(m_x1,3)+d*CppAD::pow(m_x1,2)+e*m_x1+f; // ==y1
+
+        fg[3]=5*a*CppAD::pow(m_x0,4)+4*b*CppAD::pow(m_x0,3)+3*c*CppAD::pow(m_x0,2)+2*d*m_x0+e ;  // gradient0
+        fg[4]=5*a*CppAD::pow(m_x1,4)+4*b*CppAD::pow(m_x1,3)+3*c*CppAD::pow(m_x1,2)+2*d*m_x1+e ;  // gradient0
+
+        fg[5]=10*a*CppAD::pow(m_x1,3)+6*b*CppAD::pow(m_x1,2)+3*c*m_x1+d ;         //  0
+
+
+
+    }
+};
+
+
+
+CppAD5Trajectory::CppAD5Trajectory(){}
+CppAD5Trajectory::~CppAD5Trajectory(){}
+bool CppAD5Trajectory::Make(tf::Transform t0,tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+    double angle =gen_axis_angle();
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    if(Solve(t0,t1)==false) return false;
+
+    std::cout<<"  solve param :"<<m_p;
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+    trajectory_.clear();
+    double a=m_p[0];
+    double b=m_p[1];
+    double c=m_p[2];
+    double d=m_p[3];
+    double e=m_p[4];
+    double f=m_p[5];
+    //计算两轮间距/2
+    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
+        pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
+    distance+=0.00000001;
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,5.0)+b*pow(x,4.0)+c*pow(x,3.0)+d*pow(x,2.0)+e*x+f;
+        double gradient=5.0*a*pow(x,4.0)+4.0*b*pow(x,3.0)+3.0*c*x*x+2.0*d*x+e;
+
+        //计算x处的角度变化率
+        /*double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }*/
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 5.0) + b * pow(x_last, 4.0) + c * pow(x_last,3.0)
+            + d*pow(x_last,2.0)+e*x_last+f;
+        double gradient_last = 5.0 * a * pow(x_last, 4.0) + 4.0 * b * pow(x_last,3.0)
+            + 3.0*c*pow(x_last,2.0)+2.0*d*x_last+e;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}
+
+bool CppAD5Trajectory::Solve(tf::Pose t0,tf::Pose t1)
+{
+    bool ok=true;
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    typedef CPPAD_TESTVECTOR(double) Dvector;
+    size_t n_vars = 6;
+    size_t n_constraints = 5;
+
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars_lowerbound[i] = -1.0e19;
+        vars_upperbound[i] = 1.0e19;
+    }
+
+    Dvector constraints_lowerbound(n_constraints);
+    Dvector constraints_upperbound(n_constraints);
+
+    constraints_lowerbound[0] = y0;
+    constraints_upperbound[0] = y0;
+
+    constraints_lowerbound[1] = y1;
+    constraints_upperbound[1] = y1;
+
+    constraints_lowerbound[2] = gradient1;
+    constraints_upperbound[2] = gradient1;
+
+    constraints_lowerbound[3] = gradient2;
+    constraints_upperbound[3] = gradient2;
+
+    constraints_lowerbound[4] = 0;
+    constraints_upperbound[4] = 0;
+
+    // object that computes objective and constraints
+    FG5_eval fg_eval(x0,x1);
+
+    std::string options;
+    options += "Integer print_level  0\n";
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    options += "Numeric max_cpu_time          0.5\n";
+
+    CppAD::ipopt::solve_result<Dvector> solution;
+    std::cout<<" options :"<<options;
+    CppAD::ipopt::solve<Dvector, FG5_eval>(
+        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+        constraints_upperbound, fg_eval, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+    std::cout<<" ok:"<<solution.status<<std::endl;
+    auto cost = solution.obj_value;
+
+    if(ok)
+    {
+        Eigen::VectorXd p(6);
+        p<<solution.x[0],solution.x[1],solution.x[2],solution.x[3],solution.x[4],solution.x[5];
+        std::cout<<" solve p : "<<p;
+        m_p=p;
+    }
+    return ok;
+
+}
+
+

MPC/src/trajectory/CppAD5Trajectory.h

@@ -0,0 +1,23 @@
+//
+// Created by zx on 2019/8/30.
+//
+
+#ifndef CPPAD5TRAJECTORY_H
+#define CPPAD5TRAJECTORY_H
+#include "make_trajectory.h"
+
+class CppAD5Trajectory : public MakeTrajectory
+{
+public:
+    CppAD5Trajectory();
+    ~CppAD5Trajectory();
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+protected:
+    bool Solve(tf::Pose pose1,tf::Pose pose2);
+
+protected:
+    Eigen::VectorXd   m_p;
+};
+
+
+#endif //CPPAD4TRAJECTORY_H

MPC/src/trajectory/FourthTrajectory.cpp

@@ -0,0 +1,149 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#include "FourthTrajectory.h"
+
+FourthTrajectory::FourthTrajectory()
+{
+
+}
+FourthTrajectory::~FourthTrajectory()
+{}
+
+bool FourthTrajectory::Make(tf::Transform t0, tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,5,5> A=Eigen::MatrixXd::Zero(5,5);    //系数矩阵A
+    Eigen::Matrix<double,5,1> B=Eigen::MatrixXd::Zero(5,1);    //值矩阵B
+    A(0,0)=pow(x0,4.0);
+    A(0,1)=pow(x0,3.0);
+    A(0,2)=pow(x0,2.0);
+    A(0,3)=x0;
+    A(0,4)=1.0;
+
+    A(1,0)=pow(x1,4.0);
+    A(1,1)=pow(x1,3.0);
+    A(1,2)=pow(x1,2.0);
+    A(1,3)=x1;
+    A(1,4)=1.0;
+
+    A(2,0)=4.0*pow(x0,3.0);
+    A(2,1)=3.0*x0*x0;
+    A(2,2)=2.0*x0;
+    A(2,3)=1.0;
+
+    A(3,0)=4.0*pow(x1,3.0);
+    A(3,1)=3.0*x1*x1;
+    A(3,2)=2.0*x1;
+    A(3,3)=1.0;
+
+    A(4,0)=12*pow(x1,2.0);
+    A(4,1)=6.0*x1;
+    A(4,2)=2.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    B(4,0)=0;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    double e=P(4,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,4.0)+b*pow(x,3.0)+c*x*x+d*x+e;
+        double gradient=4.0*a*pow(x,3.0)+3.0*b*x*x+2.0*c*x+d;
+
+        //计算x处的角度变化率
+        /*double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }*/
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 4.0) + b * pow(x_last, 3.0) + c * x_last*x_last + d*x_last+e;
+        double gradient_last = 4.0 * a * pow(x_last, 3.0) + 3.0 * b * x_last*x_last + 2.0*c*x_last+d;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}

MPC/src/trajectory/FourthTrajectory.h

@@ -0,0 +1,18 @@
+//
+// Created by zx on 2019/8/31.
+//
+
+#ifndef FOURTHTRAJECTORY_H
+#define FOURTHTRAJECTORY_H
+#include "make_trajectory.h"
+
+class FourthTrajectory :public MakeTrajectory
+{
+public:
+    FourthTrajectory();
+    ~FourthTrajectory();
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+};
+
+
+#endif //FOURTHTRAJECTORY_H

MPC/src/trajectory/make_trajectory.cpp

@@ -0,0 +1,282 @@
+#include "make_trajectory.h"
+#include <limits>
+
+double get_pose_angle_z(tf::Transform pos)
+{
+    double yaw,a,b;
+    pos.getBasis().getEulerZYX(yaw,a,b);
+    return yaw;
+}
+
+double get_gradient_by_tf(tf::Transform pos)
+{
+    double angle=get_pose_angle_z(pos);
+    double gradient=tanf(angle);
+    if(fabs(gradient)>200)
+        return 200.0*(gradient/fabs(gradient));
+    return gradient;
+}
+
+tf::Transform get_tf(double x,double y,double gradient)
+{
+    double angle=0;
+    if(fabs(gradient)>200.0)
+    {
+        angle=M_PI/2.0*(gradient/fabs(gradient));
+    }
+    else
+    {
+        angle=atan(fabs(gradient));
+        angle=angle*(gradient/fabs(gradient));
+    }
+    tf::Matrix3x3 rotation;
+    rotation.setRPY(0,0,angle);
+    tf::Transform tf;
+    tf.setBasis(rotation);
+    tf.setOrigin(tf::Vector3(x,y,0));
+    return tf;
+}
+
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf)
+{
+    pos=pos*tf;
+    return pos;
+}
+
+tf::Pose Rotate_pose(tf::Pose pose,double angle)
+{
+    //先平移，后旋转
+    double x=pose.getOrigin().getX();
+    double y=pose.getOrigin().getY();
+
+    double cs=cos(angle);
+    double sn=sin(angle);
+    double x1=cs*x+sn*y;
+    double y1=-sn*x+cs*y;
+    tf::Pose newpose;
+
+    tf::Quaternion quat;
+    double theta,a,b;
+    pose.getBasis().getEulerZYX(theta,a,b);
+    quat.setEulerZYX(theta-angle,a,b);
+    newpose.setRotation(quat);
+    newpose.setOrigin(tf::Vector3(x1,y1,0));
+    return newpose;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+MakeTrajectory::MakeTrajectory()
+{
+    left_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    right_tf_=tf::Transform(tf::Quaternion(0,0,0));
+    
+}
+bool MakeTrajectory::TransformTrajectory(tf::Transform tf)
+{
+
+    return true;
+}
+tf::Transform MakeTrajectory::operator[](int n)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    return trajectory_[n];
+
+}
+bool MakeTrajectory::Make(tf::Transform t0,tf::Transform t1, int extend_num)
+{
+    position0_=t0;
+    position1_=t1;
+
+    double angle =gen_axis_angle();
+
+    tf::Transform pose1=Rotate_pose(position0_,angle);
+    tf::Transform pose2=Rotate_pose(position1_,angle);
+    double gradient1=get_gradient_by_tf(pose1);
+    double gradient2=get_gradient_by_tf(pose2);
+    
+    //std::cout<<"grad1 grad2 :"<<gradient1<<" ,"<<gradient2<<std::endl;
+    //三阶多项式差值，生成 曲线
+    double x0=pose1.getOrigin().getX();
+    double y0=pose1.getOrigin().getY();
+    double x1=pose2.getOrigin().getX();
+    double y1=pose2.getOrigin().getY();
+
+    //std::cout<<" x1 ,x2 : "<<x0<<" , "<<x1<<std::endl;
+
+    Eigen::Matrix<double,4,4> A=Eigen::MatrixXd::Zero(4,4);    //系数矩阵A
+    Eigen::Matrix<double,4,1> B=Eigen::MatrixXd::Zero(4,1);    //值矩阵B
+    A(0,0)=pow(x0,3.0);
+    A(0,1)=pow(x0,2.0);
+    A(0,2)=x0;
+    A(0,3)=1.0;
+
+    A(1,0)=pow(x1,3.0);
+    A(1,1)=pow(x1,2.0);
+    A(1,2)=x1;
+    A(1,3)=1.0;
+
+    A(2,0)=3.0*pow(x0,2.0);
+    A(2,1)=2.0*x0;
+    A(2,2)=1.0;
+    
+    A(3,0)=3.0*pow(x1,2.0);
+    A(3,1)=2.0*x1;
+    A(3,2)=1.0;
+
+    B(0,0)=y0;
+    B(1,0)=y1;
+    B(2,0)=gradient1;
+    B(3,0)=gradient2;
+    Eigen::MatrixXd P=A.inverse()*B;
+    line_param_=P;
+
+    double a=P(0,0);
+    double b=P(1,0);
+    double c=P(2,0);
+    double d=P(3,0);
+    //生成离散点,间距 step (m)
+
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    trajectory_.clear();
+
+    //计算两轮间距/2
+    double distance=0.5*sqrt(pow(left_tf_.getOrigin().getX()-right_tf_.getOrigin().getX(),2.0)+
+                            pow(left_tf_.getOrigin().getY()-right_tf_.getOrigin().getY(),2.0));
+    distance+=0.00000001;
+
+    double delta=0.05;
+    double x=x0;
+    // added by yct
+    int extend_front_count = 0;
+    int extend_end_count = 0;
+    int sign = 1;
+    //modified by yct
+    while(x<x1 || (extend_num>0 && extend_end_count<=extend_num))
+    {
+        double y=a*pow(x,3.0)+b*pow(x,2.0)+c*x+d;
+        double gradient=3.0*a*pow(x,2.0)+2.0*b*x+c;
+
+        //计算x处的角度变化率
+        double gradtheta=threta_gradient(x);
+        if(fabs(gradtheta)>1.0/distance)
+        {
+            trajectory_.clear();
+            return false;
+        }
+
+        tf::Transform pose;
+        pose.setOrigin(tf::Vector3(x,y,0));
+        pose=get_tf(x,y,gradient);
+
+        // modified by yct
+        // 开头逆向移动x到扩展头，再开始入队.
+        if (extend_num > 0)
+        {
+            if(extend_front_count <= extend_num)
+            {
+                extend_front_count++;
+                sign = -1;
+            }else{
+                sign = 1;
+                trajectory_.push_back(pose);
+                if(x>=x1 && extend_end_count <= extend_num){
+                    extend_end_count++;
+                }
+            }
+        }
+        else{
+            trajectory_.push_back(pose);
+        }
+
+        double delta_x=sqrt(delta*delta/(1.0+gradient*gradient));
+        // modified by yct
+        x+=delta * sign;
+    }
+
+    //最后一点加入轨迹中, modified by yct, 非扩展手动加最后一点
+    if (extend_num <= 0)
+    {
+        double x_last = x1;
+        double y_last = a * pow(x_last, 3.0) + b * pow(x_last, 2.0) + c * x_last + d;
+        double gradient_last = 3.0 * a * pow(x_last, 2.0) + 2.0 * b * x_last + c;
+        tf::Transform pose_last;
+        pose_last.setOrigin(tf::Vector3(x_last, y_last, 0));
+        pose_last = get_tf(x_last, y_last, gradient_last);
+        trajectory_.push_back(pose_last);
+    }
+
+    //将轨迹点反变换到原坐标系
+    //tf::Transform tf_inv=axis_transform_.inverse();
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        trajectory_[i]=Rotate_pose(trajectory_[i],-angle);//trajectory_[i]*tf_inv;
+    }
+    return true;
+}
+double MakeTrajectory::gen_axis_angle()
+{
+
+    double x=position1_.getOrigin().getX()-position0_.getOrigin().getX();
+    double y=position1_.getOrigin().getY()-position0_.getOrigin().getY();
+    tf::Vector3 var(x,y,0);
+
+    double angle=atan2(y,x);
+
+    //std::cout<<" var.angle : "<<angle*180.0/M_PI<<std::endl;
+    return angle;
+
+}
+
+void MakeTrajectory::SetLRTransform(tf::Transform left,tf::Transform right)
+{
+    left_tf_=left;
+    right_tf_=right;
+}
+
+bool MakeTrajectory::IsPoseInTrajectory(tf::Transform pose)
+{
+    boost::mutex::scoped_lock lock(mutex_trajectory_);
+    if(trajectory_.size()==0)
+        return false;
+
+    double angle0=get_pose_angle_z(pose);
+    double min_distance=std::numeric_limits<double>::max();
+    tf::Transform min_point;
+    for(int i=0;i<trajectory_.size();++i)
+    {
+        tf::Transform tf=trajectory_[i];
+        double distance=sqrt(pow(pose.getOrigin().getX()-tf.getOrigin().getX(),2.0)+
+                            pow(pose.getOrigin().getY()-tf.getOrigin().getY(),2.0));
+        if(distance < min_distance)
+        {
+            min_distance=distance;
+            min_point=tf;
+        }
+    }
+    if(min_distance==std::numeric_limits<double>::max())
+        return false;
+    double angle1=get_pose_angle_z(min_point);
+    double angle_distance=fabs(angle1-angle0);
+    
+    if(min_distance>0.05 || angle_distance*180.0/M_PI>10.0)
+    {
+        std::cout<<"distance :"<<min_distance<<"   angle distance : "<<angle_distance*180.0/M_PI<<std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+double MakeTrajectory::threta_gradient(double x)
+{
+    double a=line_param_(0,0);
+    double b=line_param_(1,0);
+    double c=line_param_(2,0);
+    double d=line_param_(3,0);
+    double S=1+pow(3*a*pow(x,2.0)+2*b*x+c,2.0);
+    double s=6*a*x+2*b;
+    return s/S;
+}

MPC/src/trajectory/make_trajectory.h

@@ -0,0 +1,47 @@
+#ifndef __GEN_TRAJECTORY__
+#define __GEN_TRAJECTORY__
+/*
+ * 三次曲线获取轨迹，两点坐标+两点梯度 求曲线参数
+ */
+#include <tf/tf.h>
+#include "tf2_msgs/TFMessage.h"
+#include <boost/thread.hpp>
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+
+double get_pose_angle_z(tf::Transform pos);
+double get_gradient_by_tf(tf::Transform pos);
+tf::Transform get_tf(double x,double y,double gradient);
+tf::Transform transform_tf(tf::Transform pos,tf::Transform tf);
+tf::Pose Rotate_pose(tf::Pose pose,double angle);
+
+
+class MakeTrajectory
+{
+public:
+    MakeTrajectory();
+    bool TransformTrajectory(tf::Transform tf);
+    void SetLRTransform(tf::Transform letf,tf::Transform right);
+    tf::Transform operator[](int n);
+    int Size(){return trajectory_.size();}
+    virtual bool Make(tf::Transform t0,tf::Transform t1, int extend_num=0);
+    bool IsPoseInTrajectory(tf::Transform pose);
+protected:
+    //计算当前x点的 角度变化率
+    double threta_gradient(double x);
+    double  gen_axis_angle();
+public:
+    std::vector<tf::Transform>      trajectory_;                //雷达轨迹
+protected:
+    tf::Transform                   position0_;
+    tf::Transform                   position1_;
+
+    tf::Transform                   left_tf_;
+    tf::Transform                   right_tf_;
+    Eigen::MatrixXd                 line_param_;                //曲线参数
+
+    boost::mutex	                mutex_trajectory_;
+};
+
+#endif

cartographer_ros/.dockerignore

@@ -1,4 +0,0 @@
-**/Dockerfile*
-**/.dockerignore
-**/.git
-**/.travis.yml

cartographer_ros/.github/ISSUE_TEMPLATE

@@ -1,25 +0,0 @@
-Thank you for filing an issue!
-
-It would help us tremendously if you run through the following checklist. This
-ensures that we have the most information to quickly understand, analyze,
-reproduce and eventually resolve your issue.
-
-Please
-
-1. run `rosbag_validate` which does some checks on your sensor data. This
-   tool often finds issues that can explain poor performance and must be fixed
-   at recording time. Please post the full output of the tool into a
-   GitHub Gist at https://gist.github.com/, then link it in the issue even if
-   it does not report anything. You can run the tool like this:
-
-   rosrun cartographer_ros cartographer_rosbag_validate -bag_filename <bag filename>
-
-2. post a link to a Git repository containing a branch of
-   `cartographer_ros` containing all the configuration, launch, and URDF files
-   required to reproduce your issue.
-3. post a link to a bag file we can use to reproduce your issue. Put it on
-   Google Drive, Dropbox, any webserver or wherever it is publicly
-   downloadable.
-4. remove this boilerplate text before submitting your issue.
-
-We will likely be unable to help you without all of these points addressed.

cartographer_ros/.travis.yml

@@ -1,39 +0,0 @@
-# Copyright 2018 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.
-
-sudo: required
-services: docker
-
-# Cache intermediate Docker layers. For a description of how this works, see:
-# https://giorgos.sealabs.net/docker-cache-on-travis-and-docker-112.html
-cache:
-  directories:
-    - /home/travis/docker/
-
-env:
-  - ROS_RELEASE=indigo DOCKER_CACHE_FILE=/home/travis/docker/indigo-cache.tar.gz
-  - ROS_RELEASE=kinetic DOCKER_CACHE_FILE=/home/travis/docker/kinetic-cache.tar.gz
-  - ROS_RELEASE=lunar DOCKER_CACHE_FILE=/home/travis/docker/lunar-cache.tar.gz
-  - ROS_RELEASE=melodic DOCKER_CACHE_FILE=/home/travis/docker/melodic-cache.tar.gz
-
-before_install:
-  # $GITHUB_TOKEN must be a valid GitHub access token without access rights (https://github.com/settings/tokens).
-  # Either add your token to the 'env' section above or add it as an unencrypted variable in the Travis settings.
-  - scripts/check_access_token.sh $GITHUB_TOKEN
-  - scripts/load_docker_cache.sh
-
-install: true
-script:
-  - docker build ${TRAVIS_BUILD_DIR} -t cartographer_ros:${ROS_RELEASE} -f Dockerfile.${ROS_RELEASE} --build-arg github_token=${GITHUB_TOKEN}
-  - scripts/save_docker_cache.sh

cartographer_ros/AUTHORS

@@ -1,7 +0,0 @@
-# This is the list of Cartographer authors for copyright purposes.
-#
-# This does not necessarily list everyone who has contributed code, since in
-# some cases, their employer may be the copyright holder.  To see the full list
-# of contributors, see the revision history in source control.
-Google Inc.
-and other contributors

cartographer_ros/CONTRIBUTING.md

@@ -1,27 +0,0 @@
-Want to contribute? Great! First, read this page (including the small print at the end).
-
-### Before you contribute
-Before we can use your code, you must sign the
-[Google Individual Contributor License Agreement]
-(https://cla.developers.google.com/about/google-individual)
-(CLA), which you can do online. The CLA is necessary mainly because you own the
-copyright to your changes, even after your contribution becomes part of our
-codebase, so we need your permission to use and distribute your code. We also
-need to be sure of various other things—for instance that you'll tell us if you
-know that your code infringes on other people's patents. You don't have to sign
-the CLA until after you've submitted your code for review and a member has
-approved it, but you must do it before we can put your code into our codebase.
-Before you start working on a larger contribution, you should get in touch with
-us first through the issue tracker with your idea so that we can help out and
-possibly guide you. Coordinating up front makes it much easier to avoid
-frustration later on.
-
-### Code reviews
-All submissions, including submissions by project members, require review. We
-use Github pull requests for this purpose.
-
-### The small print
-Contributions made by corporations are covered by a different agreement than
-the one above, the
-[Software Grant and Corporate Contributor License Agreement]
-(https://cla.developers.google.com/about/google-corporate).

cartographer_ros/Dockerfile.base.melodic

@@ -1,43 +0,0 @@
-# Copyright 2018 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.
-
-FROM ros:melodic
-
-ARG CARTOGRAPHER_VERSION=master
-
-# Bionic's base image doesn't ship with sudo.
-RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
-
-# First, we invalidate the entire cache if googlecartographer/cartographer has
-# changed. This file's content changes whenever master changes. See:
-# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \
-    cartographer_ros/cartographer_version.json
-
-# wstool needs the updated rosinstall file to clone the correct repos.
-COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
-RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/prepare_catkin_workspace.sh
-
-# rosdep needs the updated package.xml files to install the correct debs.
-COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
-COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
-COPY scripts/install_debs.sh cartographer_ros/scripts/
-RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
-
-# Install proto3.
-RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
-

cartographer_ros/Dockerfile.indigo

@@ -1,76 +0,0 @@
-# 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.
-
-FROM ros:indigo
-
-ARG CARTOGRAPHER_VERSION=master
-
-# We require a GitHub access token to be passed.
-ARG github_token
-
-# First, we invalidate the entire cache if googlecartographer/cartographer has
-# changed. This file's content changes whenever master changes. See:
-# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
-    cartographer_ros/cartographer_version.json
-
-# wstool needs the updated rosinstall file to clone the correct repos.
-COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
-RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/prepare_catkin_workspace.sh
-
-# rosdep needs the updated package.xml files to install the correct debs.
-COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
-COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
-COPY scripts/install_debs.sh cartographer_ros/scripts/
-RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
-
-# Install proto3.
-RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
-
-# Build, install, and test all packages individually to allow caching. The
-# ordering of these steps must match the topological package ordering as
-# determined by Catkin.
-COPY scripts/install.sh cartographer_ros/scripts/
-COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
-
-COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
-
-RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
-
-RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
-    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
-
-COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
-
-COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
-
-COPY scripts/ros_entrypoint.sh /
-# A BTRFS bug may prevent us from cleaning up these directories.
-# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
-RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.kinetic

@@ -1,79 +0,0 @@
-# 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.
-
-FROM ros:kinetic
-
-ARG CARTOGRAPHER_VERSION=master
-
-# We require a GitHub access token to be passed.
-ARG github_token
-
-# Xenial's base image doesn't ship with sudo.
-RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
-
-# First, we invalidate the entire cache if googlecartographer/cartographer has
-# changed. This file's content changes whenever master changes. See:
-# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
-    cartographer_ros/cartographer_version.json
-
-# wstool needs the updated rosinstall file to clone the correct repos.
-COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
-RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/prepare_catkin_workspace.sh
-
-# rosdep needs the updated package.xml files to install the correct debs.
-COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
-COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
-COPY scripts/install_debs.sh cartographer_ros/scripts/
-RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
-
-# Install proto3.
-RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
-
-# Build, install, and test all packages individually to allow caching. The
-# ordering of these steps must match the topological package ordering as
-# determined by Catkin.
-COPY scripts/install.sh cartographer_ros/scripts/
-COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
-
-COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
-
-RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
-
-RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
-    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
-
-COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
-
-COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
-
-COPY scripts/ros_entrypoint.sh /
-# A BTRFS bug may prevent us from cleaning up these directories.
-# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
-RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.lunar

@@ -1,79 +0,0 @@
-# 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.
-
-FROM ros:lunar
-
-ARG CARTOGRAPHER_VERSION=master
-
-# We require a GitHub access token to be passed.
-ARG github_token
-
-# Xenial's base image doesn't ship with sudo.
-RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
-
-# First, we invalidate the entire cache if googlecartographer/cartographer has
-# changed. This file's content changes whenever master changes. See:
-# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
-    cartographer_ros/cartographer_version.json
-
-# wstool needs the updated rosinstall file to clone the correct repos.
-COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
-RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/prepare_catkin_workspace.sh
-
-# rosdep needs the updated package.xml files to install the correct debs.
-COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
-COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
-COPY scripts/install_debs.sh cartographer_ros/scripts/
-RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
-
-# Install proto3.
-RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
-
-# Build, install, and test all packages individually to allow caching. The
-# ordering of these steps must match the topological package ordering as
-# determined by Catkin.
-COPY scripts/install.sh cartographer_ros/scripts/
-COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
-
-COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
-
-RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
-
-RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
-    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
-
-COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
-
-COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
-
-COPY scripts/ros_entrypoint.sh /
-# A BTRFS bug may prevent us from cleaning up these directories.
-# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
-RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/Dockerfile.melodic

@@ -1,76 +0,0 @@
-# Copyright 2018 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.
-
-FROM eu.gcr.io/cartographer-141408/cartographer_ros_melodic_base
-
-ARG CARTOGRAPHER_VERSION=master
-
-# We require a GitHub access token to be passed.
-ARG github_token
-
-# Bionic's base image doesn't ship with sudo.
-RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
-
-# First, we invalidate the entire cache if googlecartographer/cartographer has
-# changed. This file's content changes whenever master changes. See:
-# http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
-    cartographer_ros/cartographer_version.json
-
-# wstool needs the updated rosinstall file to clone the correct repos.
-COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/update_catkin_workspace.sh cartographer_ros/scripts/
-RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/update_catkin_workspace.sh
-
-# rosdep needs the updated package.xml files to install the correct debs.
-COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
-COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
-COPY scripts/install_debs.sh cartographer_ros/scripts/
-RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
-
-# Build, install, and test all packages individually to allow caching. The
-# ordering of these steps must match the topological package ordering as
-# determined by Catkin.
-COPY scripts/install.sh cartographer_ros/scripts/
-COPY scripts/catkin_test_results.sh cartographer_ros/scripts/
-
-COPY cartographer_ros_msgs catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros_msgs \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros_msgs
-
-RUN cartographer_ros/scripts/install.sh --pkg ceres-solver
-
-RUN cartographer_ros/scripts/install.sh --pkg cartographer && \
-    cartographer_ros/scripts/install.sh --pkg cartographer --make-args test
-
-COPY cartographer_ros catkin_ws/src/cartographer_ros/cartographer_ros/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_ros && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_ros \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_ros
-
-COPY cartographer_rviz catkin_ws/src/cartographer_ros/cartographer_rviz/
-RUN cartographer_ros/scripts/install.sh --pkg cartographer_rviz && \
-    cartographer_ros/scripts/install.sh --pkg cartographer_rviz \
-        --catkin-make-args run_tests && \
-    cartographer_ros/scripts/catkin_test_results.sh build_isolated/cartographer_rviz
-
-COPY scripts/ros_entrypoint.sh /
-# A BTRFS bug may prevent us from cleaning up these directories.
-# https://btrfs.wiki.kernel.org/index.php/Problem_FAQ#I_cannot_delete_an_empty_directory
-RUN rm -rf cartographer_ros catkin_ws || true

cartographer_ros/LICENSE

@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   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.

cartographer_ros/README.rst

@@ -1,60 +0,0 @@
-.. 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.
-
-============================
-Cartographer ROS Integration
-============================
-
-|build| |docs| |license|
-
-Purpose
-=======
-
-`Cartographer`_ is a system that provides real-time simultaneous localization
-and mapping (`SLAM`_) in 2D and 3D across multiple platforms and sensor
-configurations. This project provides Cartographer's ROS integration.
-
-.. _Cartographer: https://github.com/googlecartographer/cartographer
-.. _SLAM: https://en.wikipedia.org/wiki/Simultaneous_localization_and_mapping
-
-Getting started
-===============
-
-* Learn to use Cartographer with ROS at `our Read the Docs site`_.
-* You can ask a question by `creating an issue`_.
-
-.. _our Read the Docs site: https://google-cartographer-ros.readthedocs.io
-.. _creating an issue: https://github.com/googlecartographer/cartographer_ros/issues/new?labels=question
-
-Contributing
-============
-
-You can find information about contributing to Cartographer's ROS integration
-at `our Contribution page`_.
-
-.. _our Contribution page: https://github.com/googlecartographer/cartographer_ros/blob/master/CONTRIBUTING.md
-
-.. |build| image:: https://travis-ci.org/googlecartographer/cartographer_ros.svg?branch=master
-    :alt: Build Status
-    :scale: 100%
-    :target: https://travis-ci.org/googlecartographer/cartographer_ros
-.. |docs| image:: https://readthedocs.org/projects/google-cartographer-ros/badge/?version=latest
-    :alt: Documentation Status
-    :scale: 100%
-    :target: https://google-cartographer-ros.readthedocs.io/en/latest/?badge=latest
-.. |license| image:: https://img.shields.io/badge/License-Apache%202.0-blue.svg
-     :alt: Apache 2 license.
-     :scale: 100%
-     :target: https://github.com/googlecartographer/cartographer_ros/blob/master/LICENSE
-

cartographer_ros/azure-pipelines.yml

@@ -1,70 +0,0 @@
-# Copyright 2018 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.
-
-jobs:
-- job: Build
-  pool:
-    vmImage: 'vs2017-win2016'
-  timeoutInMinutes: 360
-  steps:
-  - script: |
-      choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1
-      rem Azure VM runs out of space on C:, so use D: for ros and rosdeps
-      mkdir D:\opt && mklink /J C:\opt D:\opt
-      choco upgrade %ROS_METAPACKAGE% -y
-      choco upgrade ros-melodic-perception -y
-      robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL
-      git clone https://github.com/googlecartographer/cartographer src\cartographer
-      call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y
-    env:
-      ROS_METAPACKAGE: 'ros-melodic-desktop'
-    displayName: Install prerequisites
-
-  - script: |
-      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
-      call "C:\opt\ros\melodic\x64\setup.bat"
-      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
-      catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release
-    displayName: Build
-
-  - script: |
-      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
-      call "C:\opt\ros\melodic\x64\setup.bat"
-      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
-      cd build_isolated\cartographer\install && ctest --no-compress-output -T Test
-    displayName: Run cartographer tests
-
-  - script: |
-      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
-      call "C:\opt\ros\melodic\x64\setup.bat"
-      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
-      cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test
-    displayName: Build and run cartographer_ros tests
-    condition: always()
-
-  - script: |
-      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
-      call "C:\opt\ros\melodic\x64\setup.bat"
-      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
-      python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros
-    displayName: Convert tests to jUnit
-    condition: always()
-
-  - task: PublishTestResults@2
-    displayName: Publish test results
-    inputs:
-      testRunner: 'jUnit'
-      testResultsFiles: '**\jUnit.xml'
-      searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing'
-    condition: always()

cartographer_ros/cartographer_ros.files

@@ -1,65 +0,0 @@
-cartographer_ros/cartographer_ros/assets_writer.cc
-cartographer_ros/cartographer_ros/assets_writer.h
-cartographer_ros/cartographer_ros/assets_writer_main.cc
-cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc
-cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc
-cartographer_ros/cartographer_ros/configuration_files_test.cc
-cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc
-cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc
-cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc
-cartographer_ros/cartographer_ros/map_builder_bridge.cc
-cartographer_ros/cartographer_ros/map_builder_bridge.h
-cartographer_ros/cartographer_ros/metrics/family_factory.cc
-cartographer_ros/cartographer_ros/metrics/family_factory.h
-cartographer_ros/cartographer_ros/metrics/internal/counter.h
-cartographer_ros/cartographer_ros/metrics/internal/family.cc
-cartographer_ros/cartographer_ros/metrics/internal/family.h
-cartographer_ros/cartographer_ros/metrics/internal/gauge.h
-cartographer_ros/cartographer_ros/metrics/internal/histogram.cc
-cartographer_ros/cartographer_ros/metrics/internal/histogram.h
-cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc
-cartographer_ros/cartographer_ros/msg_conversion.cc
-cartographer_ros/cartographer_ros/msg_conversion.h
-cartographer_ros/cartographer_ros/msg_conversion_test.cc
-cartographer_ros/cartographer_ros/node.cc
-cartographer_ros/cartographer_ros/node.h
-cartographer_ros/cartographer_ros/node_constants.cc
-cartographer_ros/cartographer_ros/node_constants.h
-cartographer_ros/cartographer_ros/node_main.cc
-cartographer_ros/cartographer_ros/node_options.cc
-cartographer_ros/cartographer_ros/node_options.h
-cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
-cartographer_ros/cartographer_ros/offline_node.cc
-cartographer_ros/cartographer_ros/offline_node.h
-cartographer_ros/cartographer_ros/offline_node_main.cc
-cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc
-cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc
-cartographer_ros/cartographer_ros/playable_bag.cc
-cartographer_ros/cartographer_ros/playable_bag.h
-cartographer_ros/cartographer_ros/ros_log_sink.cc
-cartographer_ros/cartographer_ros/ros_log_sink.h
-cartographer_ros/cartographer_ros/ros_map.cc
-cartographer_ros/cartographer_ros/ros_map.h
-cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
-cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
-cartographer_ros/cartographer_ros/rosbag_validate_main.cc
-cartographer_ros/cartographer_ros/sensor_bridge.cc
-cartographer_ros/cartographer_ros/sensor_bridge.h
-cartographer_ros/cartographer_ros/start_trajectory_main.cc
-cartographer_ros/cartographer_ros/submap.cc
-cartographer_ros/cartographer_ros/submap.h
-cartographer_ros/cartographer_ros/tf_bridge.cc
-cartographer_ros/cartographer_ros/tf_bridge.h
-cartographer_ros/cartographer_ros/time_conversion.cc
-cartographer_ros/cartographer_ros/time_conversion.h
-cartographer_ros/cartographer_ros/time_conversion_test.cc
-cartographer_ros/cartographer_ros/trajectory_options.cc
-cartographer_ros/cartographer_ros/trajectory_options.h
-cartographer_ros/cartographer_ros/urdf_reader.cc
-cartographer_ros/cartographer_ros/urdf_reader.h
-cartographer_rviz/cartographer_rviz/drawable_submap.cc
-cartographer_rviz/cartographer_rviz/drawable_submap.h
-cartographer_rviz/cartographer_rviz/ogre_slice.cc
-cartographer_rviz/cartographer_rviz/ogre_slice.h
-cartographer_rviz/cartographer_rviz/submaps_display.cc
-cartographer_rviz/cartographer_rviz/submaps_display.h

cartographer_ros/cartographer_ros.rosinstall

@@ -1,3 +0,0 @@
-- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'}
-- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'}
-- git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'}

cartographer_ros/cartographer_ros/CHANGELOG.rst

@@ -1,19 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package cartographer_ros
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-1.0.0 (2018-06-01)
-----------------------
-* https://github.com/googlecartographer/cartographer_ros/compare/0.3.0...1.0.0
-
-0.3.0 (2017-11-23)
-------------------
-* https://github.com/googlecartographer/cartographer_ros/compare/0.2.0...0.3.0
-
-0.2.0 (2017-06-19)
-------------------
-* https://github.com/googlecartographer/cartographer_ros/compare/0.1.0...0.2.0
-
-0.1.0 (2017-05-18)
-------------------
-* First unstable development release

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -1,196 +0,0 @@
-# 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.
-
-cmake_minimum_required(VERSION 2.8.12)  # Ships with Ubuntu 14.04 (Trusty)
-
-project(cartographer_ros)
-
-set(PACKAGE_DEPENDENCIES
-  cartographer_ros_msgs
-  eigen_conversions
-  geometry_msgs
-  message_runtime
-  nav_msgs
-  pcl_conversions
-  rosbag
-  roscpp
-  roslib
-  sensor_msgs
-  std_msgs
-  tf2
-  tf2_eigen
-  tf2_ros
-  urdf
-  visualization_msgs
-)
-
-if(WIN32)
-  set(Boost_USE_STATIC_LIBS FALSE)
-endif()
-# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer),
-# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS,
-# which causes linking problems on windows due to shared/static Thread clashing.
-# PCL also finds Boost. Work around by moving before find_package(cartographer).
-find_package(Boost REQUIRED COMPONENTS system iostreams)
-find_package(PCL REQUIRED COMPONENTS common)
-
-find_package(cartographer REQUIRED)
-include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
-option(BUILD_GRPC "build features that require Cartographer gRPC support" false)
-google_initialize_cartographer_project()
-google_enable_testing()
-
-find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
-
-include(FindPkgConfig)
-
-find_package(Abseil REQUIRED)
-find_package(LuaGoogle REQUIRED)
-find_package(Eigen3 REQUIRED)
-
-find_package(urdfdom_headers REQUIRED)
-if(DEFINED urdfdom_headers_VERSION)
-  if(${urdfdom_headers_VERSION} GREATER 0.4.1)
-    add_definitions(-DURDFDOM_HEADERS_HAS_SHARED_PTR_DEFS)
-  endif()
-endif()
-
-include_directories(
-  ${urdfdom_headers_INCLUDE_DIRS}
-)
-
-# Override Catkin's GTest configuration to use GMock.
-set(GTEST_FOUND TRUE)
-set(GTEST_INCLUDE_DIRS ${GMOCK_INCLUDE_DIRS})
-set(GTEST_LIBRARIES ${GMOCK_LIBRARIES})
-
-catkin_package(
-  CATKIN_DEPENDS
-    ${PACKAGE_DEPENDENCIES}
-  DEPENDS
-    # TODO(damonkohler): This should be here but causes Catkin to abort because
-    # protobuf specifies a library '-lpthread' instead of just 'pthread'.
-    # CARTOGRAPHER
-    PCL
-    EIGEN3
-    Boost
-    urdfdom_headers
-  INCLUDE_DIRS "."
-  LIBRARIES ${PROJECT_NAME}
-)
-
-file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h")
-file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc")
-file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc")
-file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*")
-list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS})
-list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES})
-if (NOT ${BUILD_GRPC})
-  list(REMOVE_ITEM ALL_SRCS ${ALL_GRPC_FILES})
-  list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES})
-  list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES})
-endif()
-add_library(${PROJECT_NAME} STATIC ${ALL_SRCS})
-add_subdirectory("cartographer_ros")
-
-target_link_libraries(${PROJECT_NAME} PUBLIC cartographer)
-
-# Lua
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
-
-# PCL
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${PCL_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PUBLIC ${PCL_LIBRARIES})
-set(BLACKLISTED_PCL_DEFINITIONS " -march=native -msse4.2 -mfpmath=sse ")
-foreach(DEFINITION ${PCL_DEFINITIONS})
-  list (FIND BLACKLISTED_PCL_DEFINITIONS "${DEFINITION}" DEFINITIONS_INDEX)
-  if (${DEFINITIONS_INDEX} GREATER -1)
-    continue()
-  endif()
-  set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${DEFINITION}")
-endforeach()
-
-# Eigen
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
-  "${EIGEN3_INCLUDE_DIR}")
-
-# Boost
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
-  "${Boost_INCLUDE_DIRS}")
-target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES})
-
-# Catkin
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES})
-add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
-
-# Add the binary directory first, so that port.h is included after it has
-# been generated.
-target_include_directories(${PROJECT_NAME} PUBLIC
-    $<BUILD_INTERFACE:${PROJECT_BINARY_DIR}>
-    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}>
-    $<INSTALL_INTERFACE:include>
-)
-
-set(TARGET_COMPILE_FLAGS "${TARGET_COMPILE_FLAGS} ${GOOG_CXX_FLAGS}")
-set_target_properties(${PROJECT_NAME} PROPERTIES
-  COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
-
-if (CATKIN_ENABLE_TESTING)
-  foreach(TEST_SOURCE_FILENAME ${ALL_TESTS})
-    get_filename_component(TEST_NAME ${TEST_SOURCE_FILENAME} NAME_WE)
-    catkin_add_gtest(${TEST_NAME} ${TEST_SOURCE_FILENAME})
-    # catkin_add_gtest uses a plain (i.e. no PUBLIC/PRIVATE/INTERFACE) call to
-    # target_link_libraries. That forces us to do the same.
-    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${LUA_INCLUDE_DIR})
-    target_link_libraries(${TEST_NAME} ${LUA_LIBRARIES})
-    target_include_directories(${TEST_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
-    target_link_libraries(${TEST_NAME} ${catkin_LIBRARIES})
-    add_dependencies(${TEST_NAME} ${catkin_EXPORTED_TARGETS})
-    target_link_libraries(${TEST_NAME} cartographer)
-    target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
-    set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
-    # Needed for dynamically linked GTest on Windows.
-    if (WIN32)
-      target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY)
-    endif()
-  endforeach()
-endif()
-
-install(DIRECTORY launch urdf configuration_files
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(PROGRAMS scripts/tf_remove_frames.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-)
-
-# Install source headers.
-file(GLOB_RECURSE HDRS "cartographer_ros/*.h")
-foreach(HDR ${HDRS})
-  file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR})
-  get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY)
-  install(
-    FILES
-      ${HDR}
-    DESTINATION
-      include/${INSTALL_DIR}
-  )
-endforeach()

cartographer_ros/cartographer_ros/cartographer_ros/.clang-format

@@ -1,2 +0,0 @@
-BasedOnStyle: Google
-DerivePointerAlignment: false

cartographer_ros/cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -1,166 +0,0 @@
-# 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.
-
-google_binary(cartographer_assets_writer
-  SRCS
-    assets_writer_main.cc
-    ros_map_writing_points_processor.h
-    ros_map_writing_points_processor.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
-)
-
-install(TARGETS cartographer_node
-  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
-)
-
-install(TARGETS cartographer_offline_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  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}
-)
-
-google_binary(cartographer_occupancy_grid_node
-  SRCS
-    occupancy_grid_node_main.cc
-)
-
-install(TARGETS cartographer_occupancy_grid_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_rosbag_validate
-  SRCS
-    rosbag_validate_main.cc
-)
-
-install(TARGETS cartographer_rosbag_validate
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_pbstream_to_ros_map
-  SRCS
-    pbstream_to_ros_map_main.cc
-)
-
-install(TARGETS cartographer_pbstream_to_ros_map
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_pbstream_map_publisher
-  SRCS
-    pbstream_map_publisher_main.cc
-)
-
-install(TARGETS cartographer_pbstream_map_publisher
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_dev_pbstream_trajectories_to_rosbag
-  SRCS
-  dev/pbstream_trajectories_to_rosbag_main.cc
-)
-
-install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_dev_rosbag_publisher
-  SRCS
-  dev/rosbag_publisher_main.cc
-)
-
-install(TARGETS cartographer_dev_rosbag_publisher
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-google_binary(cartographer_dev_trajectory_comparison
-  SRCS
-  dev/trajectory_comparison_main.cc
-)
-
-install(TARGETS cartographer_dev_trajectory_comparison
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-# TODO(cschuet): Add support for shared library case.
-if (${BUILD_GRPC})
-  google_binary(cartographer_grpc_node
-    SRCS
-      cartographer_grpc/node_grpc_main.cc
-  )
-
-  install(TARGETS cartographer_grpc_node
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-
-  google_binary(cartographer_grpc_offline_node
-    SRCS
-      cartographer_grpc/offline_node_grpc_main.cc
-  )
-
-  install(TARGETS cartographer_grpc_offline_node
-    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-
-  install(PROGRAMS
-    ../scripts/cartographer_grpc_server.sh
-    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-  )
-endif()

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer.cc

@@ -1,272 +0,0 @@
-/*
- * 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 "cartographer_ros/assets_writer.h"
-
-#include <algorithm>
-#include <fstream>
-#include <iostream>
-
-#include "absl/memory/memory.h"
-#include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/common/math.h"
-#include "cartographer/io/file_writer.h"
-#include "cartographer/io/points_processor.h"
-#include "cartographer/io/points_processor_pipeline_builder.h"
-#include "cartographer/io/proto_stream.h"
-#include "cartographer/io/proto_stream_deserializer.h"
-#include "cartographer/mapping/proto/pose_graph.pb.h"
-#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
-#include "cartographer/sensor/point_cloud.h"
-#include "cartographer/sensor/range_data.h"
-#include "cartographer/transform/transform_interpolation_buffer.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/ros_map_writing_points_processor.h"
-#include "cartographer_ros/time_conversion.h"
-#include "cartographer_ros/urdf_reader.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-#include "ros/ros.h"
-#include "ros/time.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "tf2_eigen/tf2_eigen.h"
-#include "tf2_msgs/TFMessage.h"
-#include "tf2_ros/buffer.h"
-#include "urdf/model.h"
-
-namespace cartographer_ros {
-namespace {
-
-constexpr char kTfStaticTopic[] = "/tf_static";
-namespace carto = ::cartographer;
-
-std::unique_ptr<carto::io::PointsProcessorPipelineBuilder>
-CreatePipelineBuilder(
-    const std::vector<carto::mapping::proto::Trajectory>& trajectories,
-    const std::string file_prefix) {
-  const auto file_writer_factory =
-      AssetsWriter::CreateFileWriterFactory(file_prefix);
-  auto builder = absl::make_unique<carto::io::PointsProcessorPipelineBuilder>();
-  carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory,
-                                             builder.get());
-  builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName,
-                    [file_writer_factory](
-                        carto::common::LuaParameterDictionary* const dictionary,
-                        carto::io::PointsProcessor* const next)
-                        -> std::unique_ptr<carto::io::PointsProcessor> {
-                      return RosMapWritingPointsProcessor::FromDictionary(
-                          file_writer_factory, dictionary, next);
-                    });
-  return builder;
-}
-
-std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
-    const std::string& configuration_directory,
-    const std::string& configuration_basename) {
-  auto file_resolver =
-      absl::make_unique<carto::common::ConfigurationFileResolver>(
-          std::vector<std::string>{configuration_directory});
-
-  const std::string code =
-      file_resolver->GetFileContentOrDie(configuration_basename);
-  auto lua_parameter_dictionary =
-      absl::make_unique<carto::common::LuaParameterDictionary>(
-          code, std::move(file_resolver));
-  return lua_parameter_dictionary;
-}
-
-template <typename T>
-std::unique_ptr<carto::io::PointsBatch> HandleMessage(
-    const T& message, const std::string& tracking_frame,
-    const tf2_ros::Buffer& tf_buffer,
-    const carto::transform::TransformInterpolationBuffer&
-        transform_interpolation_buffer) {
-  const carto::common::Time start_time = FromRos(message.header.stamp);
-
-  auto points_batch = absl::make_unique<carto::io::PointsBatch>();
-  points_batch->start_time = start_time;
-  points_batch->frame_id = message.header.frame_id;
-
-  carto::sensor::PointCloudWithIntensities point_cloud;
-  carto::common::Time point_cloud_time;
-  std::tie(point_cloud, point_cloud_time) =
-      ToPointCloudWithIntensities(message);
-  CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size());
-
-  for (size_t i = 0; i < point_cloud.points.size(); ++i) {
-    const carto::common::Time time =
-        point_cloud_time +
-        carto::common::FromSeconds(point_cloud.points[i].time);
-    if (!transform_interpolation_buffer.Has(time)) {
-      continue;
-    }
-    const carto::transform::Rigid3d tracking_to_map =
-        transform_interpolation_buffer.Lookup(time);
-    const carto::transform::Rigid3d sensor_to_tracking =
-        ToRigid3d(tf_buffer.lookupTransform(
-            tracking_frame, message.header.frame_id, ToRos(time)));
-    const carto::transform::Rigid3f sensor_to_map =
-        (tracking_to_map * sensor_to_tracking).cast<float>();
-    points_batch->points.push_back(
-        sensor_to_map *
-        carto::sensor::ToRangefinderPoint(point_cloud.points[i]));
-    points_batch->intensities.push_back(point_cloud.intensities[i]);
-    // We use the last transform for the origin, which is approximately correct.
-    points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();
-  }
-  if (points_batch->points.empty()) {
-    return nullptr;
-  }
-  return points_batch;
-}
-
-}  // namespace
-
-AssetsWriter::AssetsWriter(const std::string& pose_graph_filename,
-                           const std::vector<std::string>& bag_filenames,
-                           const std::string& output_file_prefix)
-    : bag_filenames_(bag_filenames),
-      pose_graph_(
-          carto::io::DeserializePoseGraphFromFile(pose_graph_filename)) {
-  CHECK_EQ(pose_graph_.trajectory_size(), bag_filenames_.size())
-      << "Pose graphs contains " << pose_graph_.trajectory_size()
-      << " trajectories while " << bag_filenames_.size()
-      << " bags were provided. This tool requires one bag for each "
-         "trajectory in the same order as the correponding trajectories in the "
-         "pose graph proto.";
-
-  // This vector must outlive the pipeline.
-  all_trajectories_ = std::vector<::cartographer::mapping::proto::Trajectory>(
-      pose_graph_.trajectory().begin(), pose_graph_.trajectory().end());
-
-  const std::string file_prefix = !output_file_prefix.empty()
-                                      ? output_file_prefix
-                                      : bag_filenames_.front() + "_";
-  point_pipeline_builder_ =
-      CreatePipelineBuilder(all_trajectories_, file_prefix);
-}
-
-void AssetsWriter::RegisterPointsProcessor(
-    const std::string& name,
-    cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction factory) {
-  point_pipeline_builder_->Register(name, factory);
-}
-
-void AssetsWriter::Run(const std::string& configuration_directory,
-                       const std::string& configuration_basename,
-                       const std::string& urdf_filename,
-                       const bool use_bag_transforms) {
-  const auto lua_parameter_dictionary =
-      LoadLuaDictionary(configuration_directory, configuration_basename);
-
-  std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
-      point_pipeline_builder_->CreatePipeline(
-          lua_parameter_dictionary->GetDictionary("pipeline").get());
-  const std::string tracking_frame =
-      lua_parameter_dictionary->GetString("tracking_frame");
-
-  do {
-    for (size_t trajectory_id = 0; trajectory_id < bag_filenames_.size();
-         ++trajectory_id) {
-      const carto::mapping::proto::Trajectory& trajectory_proto =
-          pose_graph_.trajectory(trajectory_id);
-      const std::string& bag_filename = bag_filenames_[trajectory_id];
-      LOG(INFO) << "Processing " << bag_filename << "...";
-      if (trajectory_proto.node_size() == 0) {
-        continue;
-      }
-      tf2_ros::Buffer tf_buffer;
-      if (!urdf_filename.empty()) {
-        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
-      }
-
-      const carto::transform::TransformInterpolationBuffer
-          transform_interpolation_buffer(trajectory_proto);
-      rosbag::Bag bag;
-      bag.open(bag_filename, rosbag::bagmode::Read);
-      rosbag::View view(bag);
-      const ::ros::Time begin_time = view.getBeginTime();
-      const double duration_in_seconds =
-          (view.getEndTime() - begin_time).toSec();
-
-      // We need to keep 'tf_buffer' small because it becomes very inefficient
-      // otherwise. We make sure that tf_messages are published before any data
-      // messages, so that tf lookups always work.
-      std::deque<rosbag::MessageInstance> delayed_messages;
-      // We publish tf messages one second earlier than other messages. Under
-      // the assumption of higher frequency tf this should ensure that tf can
-      // always interpolate.
-      const ::ros::Duration kDelay(1.);
-      for (const rosbag::MessageInstance& message : view) {
-        if (use_bag_transforms && message.isType<tf2_msgs::TFMessage>()) {
-          auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
-          for (const auto& transform : tf_message->transforms) {
-            try {
-              tf_buffer.setTransform(transform, "unused_authority",
-                                     message.getTopic() == kTfStaticTopic);
-            } catch (const tf2::TransformException& ex) {
-              LOG(WARNING) << ex.what();
-            }
-          }
-        }
-
-        while (!delayed_messages.empty() && delayed_messages.front().getTime() <
-                                                message.getTime() - kDelay) {
-          const rosbag::MessageInstance& delayed_message =
-              delayed_messages.front();
-
-          std::unique_ptr<carto::io::PointsBatch> points_batch;
-          if (delayed_message.isType<sensor_msgs::PointCloud2>()) {
-            points_batch = HandleMessage(
-                *delayed_message.instantiate<sensor_msgs::PointCloud2>(),
-                tracking_frame, tf_buffer, transform_interpolation_buffer);
-          } else if (delayed_message
-                         .isType<sensor_msgs::MultiEchoLaserScan>()) {
-            points_batch = HandleMessage(
-                *delayed_message.instantiate<sensor_msgs::MultiEchoLaserScan>(),
-                tracking_frame, tf_buffer, transform_interpolation_buffer);
-          } else if (delayed_message.isType<sensor_msgs::LaserScan>()) {
-            points_batch = HandleMessage(
-                *delayed_message.instantiate<sensor_msgs::LaserScan>(),
-                tracking_frame, tf_buffer, transform_interpolation_buffer);
-          }
-          if (points_batch != nullptr) {
-            points_batch->trajectory_id = trajectory_id;
-            pipeline.back()->Process(std::move(points_batch));
-          }
-          delayed_messages.pop_front();
-        }
-        delayed_messages.push_back(message);
-        LOG_EVERY_N(INFO, 10000)
-            << "Processed " << (message.getTime() - begin_time).toSec()
-            << " of " << duration_in_seconds << " bag time seconds...";
-      }
-      bag.close();
-    }
-  } while (pipeline.back()->Flush() ==
-           carto::io::PointsProcessor::FlushResult::kRestartStream);
-}
-
-::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory(
-    const std::string& file_path) {
-  const auto file_writer_factory = [file_path](const std::string& filename) {
-    return absl::make_unique<carto::io::StreamFileWriter>(file_path + filename);
-  };
-  return file_writer_factory;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer.h

@@ -1,63 +0,0 @@
-/*
- * 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 <string>
-#include <vector>
-
-#include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/io/points_processor_pipeline_builder.h"
-#include "cartographer/mapping/proto/pose_graph.pb.h"
-#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
-
-#ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H
-
-namespace cartographer_ros {
-
-class AssetsWriter {
- public:
-  AssetsWriter(const std::string& pose_graph_filename,
-               const std::vector<std::string>& bag_filenames,
-               const std::string& output_file_prefix);
-
-  // Registers a new PointsProcessor type uniquly identified by 'name' which
-  // will be created using 'factory'.
-  void RegisterPointsProcessor(
-      const std::string& name,
-      cartographer::io::PointsProcessorPipelineBuilder::FactoryFunction
-          factory);
-
-  // Configures a points processing pipeline and pushes the points from the
-  // bag through the pipeline.
-  void Run(const std::string& configuration_directory,
-           const std::string& configuration_basename,
-           const std::string& urdf_filename, bool use_bag_transforms);
-
-  // Creates a FileWriterFactory which creates a FileWriter for storing assets.
-  static ::cartographer::io::FileWriterFactory CreateFileWriterFactory(
-      const std::string& file_path);
-
- private:
-  std::vector<std::string> bag_filenames_;
-  std::vector<::cartographer::mapping::proto::Trajectory> all_trajectories_;
-  ::cartographer::mapping::proto::PoseGraph pose_graph_;
-  std::unique_ptr<::cartographer::io::PointsProcessorPipelineBuilder>
-      point_pipeline_builder_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ASSETS_WRITER_H

cartographer_ros/cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -1,64 +0,0 @@
-/*
- * 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 "absl/strings/str_split.h"
-#include "cartographer_ros/assets_writer.h"
-#include "gflags/gflags.h"
-#include "glog/logging.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.");
-DEFINE_string(
-    urdf_filename, "",
-    "URDF file that contains static links for your sensor configuration.");
-DEFINE_string(bag_filenames, "",
-              "Bags to process, must be in the same order as the trajectories "
-              "in 'pose_graph_filename'.");
-DEFINE_string(pose_graph_filename, "",
-              "Proto stream file containing the pose graph.");
-DEFINE_bool(use_bag_transforms, true,
-            "Whether to read and use the transforms from the bag.");
-DEFINE_string(output_file_prefix, "",
-              "Will be prefixed to all output file names and can be used to "
-              "define the output directory. If empty, the first bag filename "
-              "will be used.");
-
-int main(int argc, char** argv) {
-  FLAGS_alsologtostderr = true;
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_configuration_directory.empty())
-      << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basename.empty())
-      << "-configuration_basename is missing.";
-  CHECK(!FLAGS_bag_filenames.empty()) << "-bag_filenames is missing.";
-  CHECK(!FLAGS_pose_graph_filename.empty())
-      << "-pose_graph_filename is missing.";
-
-  ::cartographer_ros::AssetsWriter asset_writer(
-      FLAGS_pose_graph_filename,
-      absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()),
-      FLAGS_output_file_prefix);
-
-  asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename,
-                   FLAGS_urdf_filename, FLAGS_use_bag_transforms);
-}

cartographer_ros/cartographer_ros/cartographer_ros/cartographer_grpc/node_grpc_main.cc

@@ -1,117 +0,0 @@
-/*
- * Copyright 2017 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 "absl/memory/memory.h"
-#include "cartographer/cloud/client/map_builder_stub.h"
-#include "cartographer_ros/node.h"
-#include "cartographer_ros/node_options.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "gflags/gflags.h"
-#include "tf2_ros/transform_listener.h"
-
-DEFINE_bool(collect_metrics, false,
-            "Activates the collection of runtime metrics. If activated, the "
-            "metrics can be accessed via a ROS service.");
-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.");
-DEFINE_string(server_address, "localhost:50051",
-              "gRPC server address to stream the sensor data to.");
-DEFINE_bool(
-    start_trajectory_with_default_topics, true,
-    "Enable to immediately start the first trajectory with default topics.");
-DEFINE_string(
-    save_map_filename, "",
-    "If non-empty, serialize state and write it to disk before shutting down.");
-DEFINE_string(load_state_filename, "",
-              "If non-empty, filename of a .pbstream file "
-              "to load, containing a saved SLAM state. "
-              "Unless --upload_load_state_file is set, the filepath refers "
-              "to the gRPC server's file system.");
-DEFINE_bool(load_frozen_state, true,
-            "Load the saved state as frozen (non-optimized) trajectories.");
-DEFINE_bool(upload_load_state_file, false,
-            "Upload the .pbstream file from a local path to the (remote) gRPC "
-            "server instead of loading it from the server file system.");
-DEFINE_string(client_id, "",
-              "Cartographer client ID to use when connecting to the server.");
-
-namespace cartographer_ros {
-namespace {
-
-void Run() {
-  constexpr double kTfBufferCacheTimeInSeconds = 10.;
-  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
-  tf2_ros::TransformListener tf(tf_buffer);
-  NodeOptions node_options;
-  TrajectoryOptions trajectory_options;
-  std::tie(node_options, trajectory_options) =
-      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
-
-  auto map_builder = absl::make_unique<::cartographer::cloud::MapBuilderStub>(
-      FLAGS_server_address, FLAGS_client_id);
-
-  if (!FLAGS_load_state_filename.empty() && !FLAGS_upload_load_state_file) {
-    map_builder->LoadStateFromFile(FLAGS_load_state_filename,
-                                   FLAGS_load_frozen_state);
-  }
-
-  Node node(node_options, std::move(map_builder), &tf_buffer,
-            FLAGS_collect_metrics);
-
-  if (!FLAGS_load_state_filename.empty() && FLAGS_upload_load_state_file) {
-    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
-  }
-
-  if (FLAGS_start_trajectory_with_default_topics) {
-    node.StartTrajectoryWithDefaultTopics(trajectory_options);
-  }
-
-  ::ros::spin();
-
-  node.FinishAllTrajectories();
-  node.RunFinalOptimization();
-
-  if (!FLAGS_save_map_filename.empty()) {
-    node.SerializeState(FLAGS_save_map_filename,
-                        false /* include_unfinished_submaps */);
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_configuration_directory.empty())
-      << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basename.empty())
-      << "-configuration_basename is missing.";
-  CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
-
-  ::ros::init(argc, argv, "cartographer_grpc_node");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-  cartographer_ros::Run();
-  ::ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/cartographer_grpc/offline_node_grpc_main.cc

@@ -1,49 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer/cloud/client/map_builder_stub.h"
-#include "cartographer_ros/offline_node.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "gflags/gflags.h"
-#include "ros/ros.h"
-
-DEFINE_string(server_address, "localhost:50051",
-              "gRPC server address to "
-              "stream the sensor data to.");
-DEFINE_string(client_id, "",
-              "Cartographer client ID to use when connecting to the server.");
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_client_id.empty()) << "-client_id is missing.";
-
-  ::ros::init(argc, argv, "cartographer_grpc_offline_node");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-
-  const cartographer_ros::MapBuilderFactory map_builder_factory =
-      [](const ::cartographer::mapping::proto::MapBuilderOptions&) {
-        return absl::make_unique< ::cartographer::cloud::MapBuilderStub>(
-            FLAGS_server_address, FLAGS_client_id);
-      };
-
-  cartographer_ros::RunOfflineNode(map_builder_factory);
-
-  ::ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/configuration_files_test.cc

@@ -1,44 +0,0 @@
-/*
- * 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 <string>
-#include <vector>
-
-#include "cartographer_ros/node_options.h"
-#include "gtest/gtest.h"
-#include "ros/package.h"
-
-namespace cartographer_ros {
-namespace {
-
-class ConfigurationFilesTest : public ::testing::TestWithParam<const char*> {};
-
-TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
-  EXPECT_NO_FATAL_FAILURE({
-    LoadOptions(
-        ::ros::package::getPath("cartographer_ros") + "/configuration_files",
-        GetParam());
-  });
-}
-
-INSTANTIATE_TEST_CASE_P(
-    ValidateAllNodeOptions, ConfigurationFilesTest,
-    ::testing::Values("backpack_2d.lua", "backpack_2d_localization.lua",
-                      "backpack_3d.lua", "backpack_3d_localization.lua",
-                      "pr2.lua", "revo_lds.lua", "taurob_tracker.lua"));
-
-}  // namespace
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/dev/pbstream_trajectories_to_rosbag_main.cc

@@ -1,95 +0,0 @@
-/*
- * Copyright 2019 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 <iostream>
-#include <string>
-#include <vector>
-
-#include "absl/strings/str_cat.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/time_conversion.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-#include "rosbag/bag.h"
-#include "tf2_msgs/TFMessage.h"
-
-DEFINE_string(input, "", "pbstream file to process");
-DEFINE_string(output, "", "Bag file to write to.");
-DEFINE_string(parent_frame, "map", "Frame id to use as parent frame.");
-
-namespace cartographer_ros {
-namespace {
-
-geometry_msgs::TransformStamped ToTransformStamped(
-    int64_t timestamp_uts, const std::string& parent_frame_id,
-    const std::string& child_frame_id,
-    const cartographer::transform::proto::Rigid3d& parent_T_child) {
-  static int64_t seq = 0;
-  geometry_msgs::TransformStamped xfm;
-  xfm.header.seq = ++seq;
-  xfm.header.frame_id = parent_frame_id;
-  xfm.header.stamp = cartographer_ros::ToRos(
-      ::cartographer::common::FromUniversal(timestamp_uts));
-  xfm.child_frame_id = child_frame_id;
-  xfm.transform = cartographer_ros::ToGeometryMsgTransform(
-      ::cartographer::transform::ToRigid3(parent_T_child));
-  return xfm;
-}
-
-void pbstream_trajectories_to_bag(const std::string& pbstream_filename,
-                                  const std::string& output_bag_filename,
-                                  const std::string& parent_frame_id) {
-  const auto pose_graph =
-      cartographer::io::DeserializePoseGraphFromFile(FLAGS_input);
-
-  rosbag::Bag bag(output_bag_filename, rosbag::bagmode::Write);
-  for (const auto trajectory : pose_graph.trajectory()) {
-    const auto child_frame_id =
-        absl::StrCat("trajectory_", trajectory.trajectory_id());
-    LOG(INFO) << "Writing tf for trajectory id " << trajectory.trajectory_id()
-              << " with " << trajectory.node_size() << " nodes.";
-    for (const auto& node : trajectory.node()) {
-      tf2_msgs::TFMessage tf_msg;
-      tf_msg.transforms.push_back(ToTransformStamped(
-          node.timestamp(), parent_frame_id, child_frame_id, node.pose()));
-      bag.write("/tf", tf_msg.transforms[0].header.stamp, tf_msg);
-    }
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char* argv[]) {
-  FLAGS_alsologtostderr = true;
-  google::InitGoogleLogging(argv[0]);
-  google::SetUsageMessage(
-      "\n\n"
-      "Extracts all trajectories from the pbstream and creates a bag file with "
-      "the trajectory poses stored in /tf.\nFor each trajectory, the tool will "
-      "write transforms with the tf parent_frame_id set according to the "
-      "`parent_frame` commandline flag and child_frame_id to `trajectory_i`, "
-      "with `i` corresponding to the `trajectory_id`.");
-  google::ParseCommandLineFlags(&argc, &argv, true);
-  CHECK(!FLAGS_input.empty()) << "-input pbstream is missing.";
-  CHECK(!FLAGS_output.empty()) << "-output is missing.";
-
-  cartographer_ros::pbstream_trajectories_to_bag(FLAGS_input, FLAGS_output,
-                                                 FLAGS_parent_frame);
-  return 0;
-}

cartographer_ros/cartographer_ros/cartographer_ros/dev/rosbag_publisher_main.cc

@@ -1,148 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer/common/time.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "cartographer_ros/time_conversion.h"
-#include "gflags/gflags.h"
-#include "nav_msgs/Odometry.h"
-#include "ros/ros.h"
-#include "ros/time.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "tf2_msgs/TFMessage.h"
-
-DEFINE_string(bag_filename, "", "Bag to publish.");
-
-const int kQueueSize = 1;
-
-template <typename MessagePtrType>
-void PublishWithModifiedTimestamp(MessagePtrType message,
-                                  const ros::Publisher& publisher,
-                                  ros::Duration bag_to_current) {
-  ros::Time& stamp = message->header.stamp;
-  stamp += bag_to_current;
-  publisher.publish(message);
-}
-
-template <>
-void PublishWithModifiedTimestamp<tf2_msgs::TFMessage::Ptr>(
-    tf2_msgs::TFMessage::Ptr message, const ros::Publisher& publisher,
-    ros::Duration bag_to_current) {
-  for (const auto& transform : message->transforms) {
-    ros::Time& stamp = const_cast<ros::Time&>(transform.header.stamp);
-    stamp += bag_to_current;
-  }
-  publisher.publish(message);
-}
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::SetUsageMessage(
-      "\n\n"
-      "This replays and publishes messages from a given bag file, modifying "
-      "their header timestamps to match current ROS time.\n\n"
-      "Messages are published in the same sequence and with the same delay "
-      "they were recorded."
-      "Contrary to rosbag play, it does not publish a clock, so time is"
-      "hopefully smoother and it should be possible to reproduce timing"
-      "issues.\n"
-      "It only plays message types related to Cartographer.\n");
-  google::ParseCommandLineFlags(&argc, &argv, true);
-  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
-
-  ros::init(argc, argv, "rosbag_publisher");
-  ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-
-  rosbag::Bag bag;
-  bag.open(FLAGS_bag_filename, rosbag::bagmode::Read);
-  rosbag::View view(bag);
-  ros::NodeHandle node_handle;
-  bool use_sim_time;
-  node_handle.getParam("/use_sim_time", use_sim_time);
-  if (use_sim_time) {
-    LOG(ERROR) << "use_sim_time is true but not supported. Expect conflicting "
-                  "ros::Time and message header times or weird behavior.";
-  }
-  std::map<std::string, ros::Publisher> topic_to_publisher;
-  for (const rosbag::ConnectionInfo* c : view.getConnections()) {
-    const std::string& topic = c->topic;
-    if (topic_to_publisher.count(topic) == 0) {
-      ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
-                                    c->datatype, c->msg_def);
-      topic_to_publisher[topic] = node_handle.advertise(options);
-    }
-  }
-  ros::Duration(1).sleep();
-  CHECK(ros::ok());
-
-  ros::Time current_start = ros::Time::now();
-  ros::Time bag_start = view.getBeginTime();
-  ros::Duration bag_to_current = current_start - bag_start;
-  for (const rosbag::MessageInstance& message : view) {
-    ros::Duration after_bag_start = message.getTime() - bag_start;
-    if (!::ros::ok()) {
-      break;
-    }
-    ros::Time planned_publish_time = current_start + after_bag_start;
-    ros::Time::sleepUntil(planned_publish_time);
-
-    ros::Publisher& publisher = topic_to_publisher.at(message.getTopic());
-    if (message.isType<sensor_msgs::PointCloud2>()) {
-      PublishWithModifiedTimestamp(
-          message.instantiate<sensor_msgs::PointCloud2>(), publisher,
-          bag_to_current);
-    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
-      PublishWithModifiedTimestamp(
-          message.instantiate<sensor_msgs::MultiEchoLaserScan>(), publisher,
-          bag_to_current);
-    } else if (message.isType<sensor_msgs::LaserScan>()) {
-      PublishWithModifiedTimestamp(
-          message.instantiate<sensor_msgs::LaserScan>(), publisher,
-          bag_to_current);
-    } else if (message.isType<sensor_msgs::Imu>()) {
-      PublishWithModifiedTimestamp(message.instantiate<sensor_msgs::Imu>(),
-                                   publisher, bag_to_current);
-    } else if (message.isType<nav_msgs::Odometry>()) {
-      PublishWithModifiedTimestamp(message.instantiate<nav_msgs::Odometry>(),
-                                   publisher, bag_to_current);
-    } else if (message.isType<tf2_msgs::TFMessage>()) {
-      PublishWithModifiedTimestamp(message.instantiate<tf2_msgs::TFMessage>(),
-                                   publisher, bag_to_current);
-    } else {
-      LOG(WARNING) << "Skipping message with type " << message.getDataType();
-    }
-
-    ros::Time current_time = ros::Time::now();
-    double simulation_delay = cartographer::common::ToSeconds(
-        cartographer_ros::FromRos(current_time) -
-        cartographer_ros::FromRos(planned_publish_time));
-    if (std::abs(simulation_delay) > 0.001) {
-      LOG(WARNING) << "Playback delayed by " << simulation_delay
-                   << " s. planned_publish_time: " << planned_publish_time
-                   << " current_time: " << current_time;
-    }
-  }
-  bag.close();
-
-  ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/dev/trajectory_comparison_main.cc

@@ -1,143 +0,0 @@
-/*
- * Copyright 2018 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 <algorithm>
-#include <fstream>
-#include <iostream>
-#include <string>
-#include <vector>
-
-#include "cartographer/common/math.h"
-#include "cartographer/io/proto_stream_deserializer.h"
-#include "cartographer/mapping/proto/pose_graph.pb.h"
-#include "cartographer/transform/transform_interpolation_buffer.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/time_conversion.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-#include "ros/ros.h"
-#include "ros/time.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "tf2_eigen/tf2_eigen.h"
-#include "tf2_msgs/TFMessage.h"
-
-DEFINE_string(bag_filename, "",
-              "Bag file containing TF messages of the trajectory that will be "
-              "compared against the trajectory in the .pbstream file.");
-DEFINE_string(tf_parent_frame, "map",
-              "The parent frame ID of the TF trajectory from the bag file.");
-DEFINE_string(tf_child_frame, "base_link",
-              "The child frame ID of the TF trajectory from the bag file.");
-DEFINE_string(pbstream_filename, "",
-              "Proto stream file containing the pose graph. The last "
-              "trajectory will be used for comparison.");
-
-namespace cartographer_ros {
-namespace {
-
-double FractionSmallerThan(const std::vector<double>& v, double x) {
-  return static_cast<double>(std::count_if(
-             v.begin(), v.end(), [=](double value) { return value < x; })) /
-         v.size();
-}
-
-std::string QuantilesToString(std::vector<double>* v) {
-  if (v->empty()) return "(empty vector)";
-  std::sort(v->begin(), v->end());
-  std::stringstream result;
-  const int kNumQuantiles = 10;
-  for (int i = 0; i < kNumQuantiles; ++i) {
-    auto value = v->at(v->size() * i / kNumQuantiles);
-    auto percentage = 100 * i / kNumQuantiles;
-    result << percentage << "%: " << value << "\n";
-  }
-  result << "100%: " << v->back() << "\n";
-  return result.str();
-}
-
-void Run(const std::string& pbstream_filename,
-         const std::string& bag_filename) {
-  cartographer::mapping::proto::PoseGraph pose_graph_proto =
-      cartographer::io::DeserializePoseGraphFromFile(pbstream_filename);
-  const cartographer::mapping::proto::Trajectory& last_trajectory_proto =
-      *pose_graph_proto.mutable_trajectory()->rbegin();
-  const cartographer::transform::TransformInterpolationBuffer
-      transform_interpolation_buffer(last_trajectory_proto);
-
-  rosbag::Bag bag;
-  bag.open(bag_filename, rosbag::bagmode::Read);
-  rosbag::View view(bag);
-  std::vector<double> deviation_translation, deviation_rotation;
-  const double signal_maximum = std::numeric_limits<double>::max();
-  for (const rosbag::MessageInstance& message : view) {
-    if (!message.isType<tf2_msgs::TFMessage>()) {
-      continue;
-    }
-    auto tf_message = message.instantiate<tf2_msgs::TFMessage>();
-    for (const auto& transform : tf_message->transforms) {
-      if (transform.header.frame_id != FLAGS_tf_parent_frame ||
-          transform.child_frame_id != FLAGS_tf_child_frame) {
-        continue;
-      }
-      const cartographer::common::Time transform_time =
-          FromRos(message.getTime());
-      if (!transform_interpolation_buffer.Has(transform_time)) {
-        deviation_translation.push_back(signal_maximum);
-        deviation_rotation.push_back(signal_maximum);
-        continue;
-      }
-      auto optimized_transform =
-          transform_interpolation_buffer.Lookup(transform_time);
-      auto published_transform = ToRigid3d(transform);
-      deviation_translation.push_back((published_transform.translation() -
-                                       optimized_transform.translation())
-                                          .norm());
-      deviation_rotation.push_back(
-          published_transform.rotation().angularDistance(
-              optimized_transform.rotation()));
-    }
-  }
-  bag.close();
-  LOG(INFO) << "Distribution of translation difference:\n"
-            << QuantilesToString(&deviation_translation);
-  LOG(INFO) << "Distribution of rotation difference:\n"
-            << QuantilesToString(&deviation_rotation);
-  LOG(INFO) << "Fraction of translation difference smaller than 1m: "
-            << FractionSmallerThan(deviation_translation, 1);
-  LOG(INFO) << "Fraction of translation difference smaller than 0.1m: "
-            << FractionSmallerThan(deviation_translation, 0.1);
-  LOG(INFO) << "Fraction of translation difference smaller than 0.05m: "
-            << FractionSmallerThan(deviation_translation, 0.05);
-  LOG(INFO) << "Fraction of translation difference smaller than 0.01m: "
-            << FractionSmallerThan(deviation_translation, 0.01);
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  FLAGS_alsologtostderr = true;
-  google::InitGoogleLogging(argv[0]);
-  google::SetUsageMessage(
-      "\n\n"
-      "This compares a trajectory from a bag file against the "
-      "last trajectory in a pbstream file.\n");
-  google::ParseCommandLineFlags(&argc, &argv, true);
-  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
-  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
-  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_bag_filename);
-}

cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -1,513 +0,0 @@
-/*
- * 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 "cartographer_ros/map_builder_bridge.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer/io/color.h"
-#include "cartographer/io/proto_stream.h"
-#include "cartographer/mapping/pose_graph.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros_msgs/StatusCode.h"
-#include "cartographer_ros_msgs/StatusResponse.h"
-
-namespace cartographer_ros {
-namespace {
-
-using ::cartographer::transform::Rigid3d;
-
-constexpr double kTrajectoryLineStripMarkerScale = 0.07;
-constexpr double kLandmarkMarkerScale = 0.2;
-constexpr double kConstraintMarkerScale = 0.025;
-
-::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
-  ::std_msgs::ColorRGBA result;
-  result.r = color[0];
-  result.g = color[1];
-  result.b = color[2];
-  result.a = 1.f;
-  return result;
-}
-
-visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
-                                                  const std::string& frame_id) {
-  visualization_msgs::Marker marker;
-  marker.ns = "Trajectory " + std::to_string(trajectory_id);
-  marker.id = 0;
-  marker.type = visualization_msgs::Marker::LINE_STRIP;
-  marker.header.stamp = ::ros::Time::now();
-  marker.header.frame_id = frame_id;
-  marker.color = ToMessage(cartographer::io::GetColor(trajectory_id));
-  marker.scale.x = kTrajectoryLineStripMarkerScale;
-  marker.pose.orientation.w = 1.;
-  marker.pose.position.z = 0.05;
-  return marker;
-}
-
-int GetLandmarkIndex(
-    const std::string& landmark_id,
-    std::unordered_map<std::string, int>* landmark_id_to_index) {
-  auto it = landmark_id_to_index->find(landmark_id);
-  if (it == landmark_id_to_index->end()) {
-    const int new_index = landmark_id_to_index->size();
-    landmark_id_to_index->emplace(landmark_id, new_index);
-    return new_index;
-  }
-  return it->second;
-}
-
-visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
-                                                const Rigid3d& landmark_pose,
-                                                const std::string& frame_id) {
-  visualization_msgs::Marker marker;
-  marker.ns = "Landmarks";
-  marker.id = landmark_index;
-  marker.type = visualization_msgs::Marker::SPHERE;
-  marker.header.stamp = ::ros::Time::now();
-  marker.header.frame_id = frame_id;
-  marker.scale.x = kLandmarkMarkerScale;
-  marker.scale.y = kLandmarkMarkerScale;
-  marker.scale.z = kLandmarkMarkerScale;
-  marker.color = ToMessage(cartographer::io::GetColor(landmark_index));
-  marker.pose = ToGeometryMsgPose(landmark_pose);
-  return marker;
-}
-
-void PushAndResetLineMarker(visualization_msgs::Marker* marker,
-                            std::vector<visualization_msgs::Marker>* markers) {
-  markers->push_back(*marker);
-  ++marker->id;
-  marker->points.clear();
-}
-
-}  // namespace
-
-MapBuilderBridge::MapBuilderBridge(
-    const NodeOptions& node_options,
-    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-    tf2_ros::Buffer* const tf_buffer)
-    : node_options_(node_options),
-      map_builder_(std::move(map_builder)),
-      tf_buffer_(tf_buffer) {}
-
-void MapBuilderBridge::LoadState(const std::string& state_filename,
-                                 bool load_frozen_state) {
-  // Check if suffix of the state file is ".pbstream".
-  const std::string suffix = ".pbstream";
-  CHECK_EQ(state_filename.substr(
-               std::max<int>(state_filename.size() - suffix.size(), 0)),
-           suffix)
-      << "The file containing the state to be loaded must be a "
-         ".pbstream file.";
-  LOG(INFO) << "Loading saved state '" << state_filename << "'...";
-  cartographer::io::ProtoStreamReader stream(state_filename);
-  map_builder_->LoadState(&stream, load_frozen_state);
-}
-
-int MapBuilderBridge::AddTrajectory(const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids,
-    const TrajectoryOptions& trajectory_options) 
-{
-    ///添加新轨迹
-  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
-      expected_sensor_ids, trajectory_options.trajectory_builder_options,
-      [this](const int trajectory_id, const ::cartographer::common::Time time,
-             const Rigid3d local_pose,
-             ::cartographer::sensor::RangeData range_data_in_local,
-             const std::unique_ptr<
-                 const ::cartographer::mapping::TrajectoryBuilderInterface::
-                     InsertionResult>) {
-        OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
-      });
-  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
-
-  // Make sure there is no trajectory with 'trajectory_id' yet.
-  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
-  sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
-      trajectory_options.num_subdivisions_per_laser_scan,
-      trajectory_options.tracking_frame,
-      node_options_.lookup_transform_timeout_sec, tf_buffer_,
-      map_builder_->GetTrajectoryBuilder(trajectory_id));
-  auto emplace_result =
-      trajectory_options_.emplace(trajectory_id, trajectory_options);
-  CHECK(emplace_result.second == true);
-  return trajectory_id;
-}
-
-void MapBuilderBridge::FinishTrajectory(const int trajectory_id) {
-  LOG(INFO) << "Finishing trajectory with ID '" << trajectory_id << "'...";
-
-  // Make sure there is a trajectory with 'trajectory_id'.
-  CHECK(GetTrajectoryStates().count(trajectory_id));
-  map_builder_->FinishTrajectory(trajectory_id);
-  sensor_bridges_.erase(trajectory_id);
-}
-
-void MapBuilderBridge::RunFinalOptimization() {
-  LOG(INFO) << "Running final trajectory optimization...";
-  map_builder_->pose_graph()->RunFinalOptimization();
-}
-
-bool MapBuilderBridge::SerializeState(const std::string& filename,
-                                      const bool include_unfinished_submaps) {
-  return map_builder_->SerializeStateToFile(include_unfinished_submaps,
-                                            filename);
-}
-
-void MapBuilderBridge::HandleSubmapQuery(
-    cartographer_ros_msgs::SubmapQuery::Request& request,
-    cartographer_ros_msgs::SubmapQuery::Response& response) {
-  cartographer::mapping::proto::SubmapQuery::Response response_proto;
-  cartographer::mapping::SubmapId submap_id{request.trajectory_id,
-                                            request.submap_index};
-  const std::string error =
-      map_builder_->SubmapToProto(submap_id, &response_proto);
-  if (!error.empty()) {
-    LOG(ERROR) << error;
-    response.status.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
-    response.status.message = error;
-    return;
-  }
-
-  response.submap_version = response_proto.submap_version();
-  for (const auto& texture_proto : response_proto.textures()) {
-    response.textures.emplace_back();
-    auto& texture = response.textures.back();
-    texture.cells.insert(texture.cells.begin(), texture_proto.cells().begin(),
-                         texture_proto.cells().end());
-    texture.width = texture_proto.width();
-    texture.height = texture_proto.height();
-    texture.resolution = texture_proto.resolution();
-    texture.slice_pose = ToGeometryMsgPose(
-        cartographer::transform::ToRigid3(texture_proto.slice_pose()));
-  }
-  response.status.message = "Success.";
-  response.status.code = cartographer_ros_msgs::StatusCode::OK;
-}
-
-std::map<int, ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
-MapBuilderBridge::GetTrajectoryStates() {
-  auto trajectory_states = map_builder_->pose_graph()->GetTrajectoryStates();
-  // Add active trajectories that are not yet in the pose graph, but are e.g.
-  // waiting for input sensor data and thus already have a sensor bridge.
-  for (const auto& sensor_bridge : sensor_bridges_) {
-    trajectory_states.insert(std::make_pair(
-        sensor_bridge.first,
-        ::cartographer::mapping::PoseGraph::TrajectoryState::ACTIVE));
-  }
-  return trajectory_states;
-}
-
-cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
-  cartographer_ros_msgs::SubmapList submap_list;
-  submap_list.header.stamp = ::ros::Time::now();
-  submap_list.header.frame_id = node_options_.map_frame;
-  for (const auto& submap_id_pose :
-       map_builder_->pose_graph()->GetAllSubmapPoses()) {
-    cartographer_ros_msgs::SubmapEntry submap_entry;
-    submap_entry.is_frozen = map_builder_->pose_graph()->IsTrajectoryFrozen(
-        submap_id_pose.id.trajectory_id);
-    submap_entry.trajectory_id = submap_id_pose.id.trajectory_id;
-    submap_entry.submap_index = submap_id_pose.id.submap_index;
-    submap_entry.submap_version = submap_id_pose.data.version;
-    submap_entry.pose = ToGeometryMsgPose(submap_id_pose.data.pose);
-    submap_list.submap.push_back(submap_entry);
-  }
-  return submap_list;
-}
-
-std::unordered_map<int, MapBuilderBridge::LocalTrajectoryData>
-MapBuilderBridge::GetLocalTrajectoryData() {
-  std::unordered_map<int, LocalTrajectoryData> local_trajectory_data;
-  for (const auto& entry : sensor_bridges_) {
-    const int trajectory_id = entry.first;
-    const SensorBridge& sensor_bridge = *entry.second;
-
-    std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data;
-    {
-      absl::MutexLock lock(&mutex_);
-      if (local_slam_data_.count(trajectory_id) == 0) {
-        continue;
-      }
-      local_slam_data = local_slam_data_.at(trajectory_id);
-    }
-
-    // Make sure there is a trajectory with 'trajectory_id'.
-    CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
-    local_trajectory_data[trajectory_id] = {
-        local_slam_data,
-        map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
-        sensor_bridge.tf_bridge().LookupToTracking(
-            local_slam_data->time,
-            trajectory_options_[trajectory_id].published_frame),
-        trajectory_options_[trajectory_id]};
-  }
-  return local_trajectory_data;
-}
-
-visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {
-  visualization_msgs::MarkerArray trajectory_node_list;
-  const auto node_poses = map_builder_->pose_graph()->GetTrajectoryNodePoses();
-  // Find the last node indices for each trajectory that have either
-  // inter-submap or inter-trajectory constraints.
-  std::map<int, int /* node_index */>
-      trajectory_to_last_inter_submap_constrained_node;
-  std::map<int, int /* node_index */>
-      trajectory_to_last_inter_trajectory_constrained_node;
-  for (const int trajectory_id : node_poses.trajectory_ids()) {
-    trajectory_to_last_inter_submap_constrained_node[trajectory_id] = 0;
-    trajectory_to_last_inter_trajectory_constrained_node[trajectory_id] = 0;
-  }
-  const auto constraints = map_builder_->pose_graph()->constraints();
-  for (const auto& constraint : constraints) {
-    if (constraint.tag ==
-        cartographer::mapping::PoseGraph::Constraint::INTER_SUBMAP) {
-      if (constraint.node_id.trajectory_id ==
-          constraint.submap_id.trajectory_id) {
-        trajectory_to_last_inter_submap_constrained_node[constraint.node_id
-                                                             .trajectory_id] =
-            std::max(trajectory_to_last_inter_submap_constrained_node.at(
-                         constraint.node_id.trajectory_id),
-                     constraint.node_id.node_index);
-      } else {
-        trajectory_to_last_inter_trajectory_constrained_node
-            [constraint.node_id.trajectory_id] =
-                std::max(trajectory_to_last_inter_submap_constrained_node.at(
-                             constraint.node_id.trajectory_id),
-                         constraint.node_id.node_index);
-      }
-    }
-  }
-
-  for (const int trajectory_id : node_poses.trajectory_ids()) {
-    visualization_msgs::Marker marker =
-        CreateTrajectoryMarker(trajectory_id, node_options_.map_frame);
-    int last_inter_submap_constrained_node = std::max(
-        node_poses.trajectory(trajectory_id).begin()->id.node_index,
-        trajectory_to_last_inter_submap_constrained_node.at(trajectory_id));
-    int last_inter_trajectory_constrained_node = std::max(
-        node_poses.trajectory(trajectory_id).begin()->id.node_index,
-        trajectory_to_last_inter_trajectory_constrained_node.at(trajectory_id));
-    last_inter_submap_constrained_node =
-        std::max(last_inter_submap_constrained_node,
-                 last_inter_trajectory_constrained_node);
-
-    if (map_builder_->pose_graph()->IsTrajectoryFrozen(trajectory_id)) {
-      last_inter_submap_constrained_node =
-          (--node_poses.trajectory(trajectory_id).end())->id.node_index;
-      last_inter_trajectory_constrained_node =
-          last_inter_submap_constrained_node;
-    }
-
-    marker.color.a = 1.0;
-    for (const auto& node_id_data : node_poses.trajectory(trajectory_id)) {
-      if (!node_id_data.data.constant_pose_data.has_value()) {
-        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
-        continue;
-      }
-      const ::geometry_msgs::Point node_point =
-          ToGeometryMsgPoint(node_id_data.data.global_pose.translation());
-      marker.points.push_back(node_point);
-
-      if (node_id_data.id.node_index ==
-          last_inter_trajectory_constrained_node) {
-        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
-        marker.points.push_back(node_point);
-        marker.color.a = 0.5;
-      }
-      if (node_id_data.id.node_index == last_inter_submap_constrained_node) {
-        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
-        marker.points.push_back(node_point);
-        marker.color.a = 0.25;
-      }
-      // Work around the 16384 point limit in RViz by splitting the
-      // trajectory into multiple markers.
-      if (marker.points.size() == 16384) {
-        PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
-        // Push back the last point, so the two markers appear connected.
-        marker.points.push_back(node_point);
-      }
-    }
-    PushAndResetLineMarker(&marker, &trajectory_node_list.markers);
-    size_t current_last_marker_id = static_cast<size_t>(marker.id - 1);
-    if (trajectory_to_highest_marker_id_.count(trajectory_id) == 0) {
-      trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
-    } else {
-      marker.action = visualization_msgs::Marker::DELETE;
-      while (static_cast<size_t>(marker.id) <=
-             trajectory_to_highest_marker_id_[trajectory_id]) {
-        trajectory_node_list.markers.push_back(marker);
-        ++marker.id;
-      }
-      trajectory_to_highest_marker_id_[trajectory_id] = current_last_marker_id;
-    }
-  }
-  return trajectory_node_list;
-}
-
-visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {
-  visualization_msgs::MarkerArray landmark_poses_list;
-  const std::map<std::string, Rigid3d> landmark_poses =
-      map_builder_->pose_graph()->GetLandmarkPoses();
-  for (const auto& id_to_pose : landmark_poses) {
-    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
-        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
-        id_to_pose.second, node_options_.map_frame));
-  }
-  return landmark_poses_list;
-}
-
-visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {
-  visualization_msgs::MarkerArray constraint_list;
-  int marker_id = 0;
-  visualization_msgs::Marker constraint_intra_marker;
-  constraint_intra_marker.id = marker_id++;
-  constraint_intra_marker.ns = "Intra constraints";
-  constraint_intra_marker.type = visualization_msgs::Marker::LINE_LIST;
-  constraint_intra_marker.header.stamp = ros::Time::now();
-  constraint_intra_marker.header.frame_id = node_options_.map_frame;
-  constraint_intra_marker.scale.x = kConstraintMarkerScale;
-  constraint_intra_marker.pose.orientation.w = 1.0;
-
-  visualization_msgs::Marker residual_intra_marker = constraint_intra_marker;
-  residual_intra_marker.id = marker_id++;
-  residual_intra_marker.ns = "Intra residuals";
-  // This and other markers which are less numerous are set to be slightly
-  // above the intra constraints marker in order to ensure that they are
-  // visible.
-  residual_intra_marker.pose.position.z = 0.1;
-
-  visualization_msgs::Marker constraint_inter_same_trajectory_marker =
-      constraint_intra_marker;
-  constraint_inter_same_trajectory_marker.id = marker_id++;
-  constraint_inter_same_trajectory_marker.ns =
-      "Inter constraints, same trajectory";
-  constraint_inter_same_trajectory_marker.pose.position.z = 0.1;
-
-  visualization_msgs::Marker residual_inter_same_trajectory_marker =
-      constraint_intra_marker;
-  residual_inter_same_trajectory_marker.id = marker_id++;
-  residual_inter_same_trajectory_marker.ns = "Inter residuals, same trajectory";
-  residual_inter_same_trajectory_marker.pose.position.z = 0.1;
-
-  visualization_msgs::Marker constraint_inter_diff_trajectory_marker =
-      constraint_intra_marker;
-  constraint_inter_diff_trajectory_marker.id = marker_id++;
-  constraint_inter_diff_trajectory_marker.ns =
-      "Inter constraints, different trajectories";
-  constraint_inter_diff_trajectory_marker.pose.position.z = 0.1;
-
-  visualization_msgs::Marker residual_inter_diff_trajectory_marker =
-      constraint_intra_marker;
-  residual_inter_diff_trajectory_marker.id = marker_id++;
-  residual_inter_diff_trajectory_marker.ns =
-      "Inter residuals, different trajectories";
-  residual_inter_diff_trajectory_marker.pose.position.z = 0.1;
-
-  const auto trajectory_node_poses =
-      map_builder_->pose_graph()->GetTrajectoryNodePoses();
-  const auto submap_poses = map_builder_->pose_graph()->GetAllSubmapPoses();
-  const auto constraints = map_builder_->pose_graph()->constraints();
-
-  for (const auto& constraint : constraints) {
-    visualization_msgs::Marker *constraint_marker, *residual_marker;
-    std_msgs::ColorRGBA color_constraint, color_residual;
-    if (constraint.tag ==
-        cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
-      constraint_marker = &constraint_intra_marker;
-      residual_marker = &residual_intra_marker;
-      // Color mapping for submaps of various trajectories - add trajectory id
-      // to ensure different starting colors. Also add a fixed offset of 25
-      // to avoid having identical colors as trajectories.
-      color_constraint = ToMessage(
-          cartographer::io::GetColor(constraint.submap_id.submap_index +
-                                     constraint.submap_id.trajectory_id + 25));
-      color_residual.a = 1.0;
-      color_residual.r = 1.0;
-    } else {
-      if (constraint.node_id.trajectory_id ==
-          constraint.submap_id.trajectory_id) {
-        constraint_marker = &constraint_inter_same_trajectory_marker;
-        residual_marker = &residual_inter_same_trajectory_marker;
-        // Bright yellow
-        color_constraint.a = 1.0;
-        color_constraint.r = color_constraint.g = 1.0;
-      } else {
-        constraint_marker = &constraint_inter_diff_trajectory_marker;
-        residual_marker = &residual_inter_diff_trajectory_marker;
-        // Bright orange
-        color_constraint.a = 1.0;
-        color_constraint.r = 1.0;
-        color_constraint.g = 165. / 255.;
-      }
-      // Bright cyan
-      color_residual.a = 1.0;
-      color_residual.b = color_residual.g = 1.0;
-    }
-
-    for (int i = 0; i < 2; ++i) {
-      constraint_marker->colors.push_back(color_constraint);
-      residual_marker->colors.push_back(color_residual);
-    }
-
-    const auto submap_it = submap_poses.find(constraint.submap_id);
-    if (submap_it == submap_poses.end()) {
-      continue;
-    }
-    const auto& submap_pose = submap_it->data.pose;
-    const auto node_it = trajectory_node_poses.find(constraint.node_id);
-    if (node_it == trajectory_node_poses.end()) {
-      continue;
-    }
-    const auto& trajectory_node_pose = node_it->data.global_pose;
-    const Rigid3d constraint_pose = submap_pose * constraint.pose.zbar_ij;
-
-    constraint_marker->points.push_back(
-        ToGeometryMsgPoint(submap_pose.translation()));
-    constraint_marker->points.push_back(
-        ToGeometryMsgPoint(constraint_pose.translation()));
-
-    residual_marker->points.push_back(
-        ToGeometryMsgPoint(constraint_pose.translation()));
-    residual_marker->points.push_back(
-        ToGeometryMsgPoint(trajectory_node_pose.translation()));
-  }
-
-  constraint_list.markers.push_back(constraint_intra_marker);
-  constraint_list.markers.push_back(residual_intra_marker);
-  constraint_list.markers.push_back(constraint_inter_same_trajectory_marker);
-  constraint_list.markers.push_back(residual_inter_same_trajectory_marker);
-  constraint_list.markers.push_back(constraint_inter_diff_trajectory_marker);
-  constraint_list.markers.push_back(residual_inter_diff_trajectory_marker);
-  return constraint_list;
-}
-
-SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
-  return sensor_bridges_.at(trajectory_id).get();
-}
-
-void MapBuilderBridge::OnLocalSlamResult(
-    const int trajectory_id, const ::cartographer::common::Time time,
-    const Rigid3d local_pose,
-    ::cartographer::sensor::RangeData range_data_in_local) {
-  std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data =
-      std::make_shared<LocalTrajectoryData::LocalSlamData>(
-          LocalTrajectoryData::LocalSlamData{time, local_pose,
-                                             std::move(range_data_in_local)});
-  absl::MutexLock lock(&mutex_);
-  local_slam_data_[trajectory_id] = std::move(local_slam_data);
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -1,124 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H
-
-#include <memory>
-#include <set>
-#include <string>
-#include <unordered_map>
-
-#include "absl/synchronization/mutex.h"
-#include "cartographer/mapping/map_builder_interface.h"
-#include "cartographer/mapping/pose_graph_interface.h"
-#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
-#include "cartographer/mapping/trajectory_builder_interface.h"
-#include "cartographer_ros/node_options.h"
-#include "cartographer_ros/sensor_bridge.h"
-#include "cartographer_ros/tf_bridge.h"
-#include "cartographer_ros/trajectory_options.h"
-#include "cartographer_ros_msgs/SubmapEntry.h"
-#include "cartographer_ros_msgs/SubmapList.h"
-#include "cartographer_ros_msgs/SubmapQuery.h"
-#include "nav_msgs/OccupancyGrid.h"
-
-// Abseil unfortunately pulls in winnt.h, which #defines DELETE.
-// Clean up to unbreak visualization_msgs::Marker::DELETE.
-#ifdef DELETE
-#undef DELETE
-#endif
-#include "visualization_msgs/MarkerArray.h"
-
-namespace cartographer_ros {
-
-class MapBuilderBridge {
- public:
-  struct LocalTrajectoryData 
-  {
-    // Contains the trajectory data received from local SLAM, after
-    // it had processed accumulated 'range_data_in_local' and estimated
-    // current 'local_pose' at 'time'.
-    struct LocalSlamData
-    {
-      ::cartographer::common::Time time;
-      ::cartographer::transform::Rigid3d local_pose;
-      ::cartographer::sensor::RangeData range_data_in_local;
-    };
-    std::shared_ptr<const LocalSlamData> local_slam_data;
-    cartographer::transform::Rigid3d local_to_map;
-    std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
-    TrajectoryOptions trajectory_options;
-  };
-
-  MapBuilderBridge(
-      const NodeOptions& node_options,
-      std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-      tf2_ros::Buffer* tf_buffer);
-
-  MapBuilderBridge(const MapBuilderBridge&) = delete;
-  MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
-
-  void LoadState(const std::string& state_filename, bool load_frozen_state);
-  int AddTrajectory(const std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>& expected_sensor_ids, 
-		    const TrajectoryOptions& trajectory_options);
-  void FinishTrajectory(int trajectory_id);
-  void RunFinalOptimization();
-  bool SerializeState(const std::string& filename,
-                      const bool include_unfinished_submaps);
-
-  void HandleSubmapQuery(
-      cartographer_ros_msgs::SubmapQuery::Request& request,
-      cartographer_ros_msgs::SubmapQuery::Response& response);
-
-  std::map<int /* trajectory_id */,
-           ::cartographer::mapping::PoseGraphInterface::TrajectoryState>
-  GetTrajectoryStates();
-  cartographer_ros_msgs::SubmapList GetSubmapList();
-  std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
-      LOCKS_EXCLUDED(mutex_);
-  visualization_msgs::MarkerArray GetTrajectoryNodeList();
-  visualization_msgs::MarkerArray GetLandmarkPosesList();
-  visualization_msgs::MarkerArray GetConstraintList();
-
-  SensorBridge* sensor_bridge(int trajectory_id);
-
- private:
-  void OnLocalSlamResult(const int trajectory_id,
-                         const ::cartographer::common::Time time,
-                         const ::cartographer::transform::Rigid3d local_pose,
-                         ::cartographer::sensor::RangeData range_data_in_local)
-      LOCKS_EXCLUDED(mutex_);
-
-  absl::Mutex mutex_;
-  const NodeOptions node_options_;
-  std::unordered_map<int,
-                     std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>
-      local_slam_data_ GUARDED_BY(mutex_);
-  std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;
-  tf2_ros::Buffer* const tf_buffer_;
-
-  std::unordered_map<std::string /* landmark ID */, int> landmark_to_index_;
-
-  // These are keyed with 'trajectory_id'.
-  std::unordered_map<int, TrajectoryOptions> trajectory_options_;
-  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
-  std::unordered_map<int, size_t> trajectory_to_highest_marker_id_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/family_factory.cc

@@ -1,69 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer_ros/metrics/family_factory.h"
-
-#include "absl/memory/memory.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
-
-::cartographer::metrics::Family<::cartographer::metrics::Counter>*
-FamilyFactory::NewCounterFamily(const std::string& name,
-                                const std::string& description) {
-  auto wrapper = absl::make_unique<CounterFamily>(name, description);
-  auto* ptr = wrapper.get();
-  counter_families_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
-FamilyFactory::NewGaugeFamily(const std::string& name,
-                              const std::string& description) {
-  auto wrapper = absl::make_unique<GaugeFamily>(name, description);
-  auto* ptr = wrapper.get();
-  gauge_families_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
-FamilyFactory::NewHistogramFamily(const std::string& name,
-                                  const std::string& description,
-                                  const BucketBoundaries& boundaries) {
-  auto wrapper =
-      absl::make_unique<HistogramFamily>(name, description, boundaries);
-  auto* ptr = wrapper.get();
-  histogram_families_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-void FamilyFactory::ReadMetrics(
-    ::cartographer_ros_msgs::ReadMetrics::Response* response) const {
-  for (const auto& counter_family : counter_families_) {
-    response->metric_families.push_back(counter_family->ToRosMessage());
-  }
-  for (const auto& gauge_family : gauge_families_) {
-    response->metric_families.push_back(gauge_family->ToRosMessage());
-  }
-  for (const auto& histogram_family : histogram_families_) {
-    response->metric_families.push_back(histogram_family->ToRosMessage());
-  }
-}
-
-}  // namespace metrics
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/family_factory.h

@@ -1,61 +0,0 @@
-/*
- * Copyright 2018 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_METRICS_FAMILY_FACTORY_H
-#define CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H
-
-#include <memory>
-#include <string>
-
-#include "cartographer/metrics/family_factory.h"
-#include "cartographer_ros/metrics/internal/counter.h"
-#include "cartographer_ros/metrics/internal/family.h"
-#include "cartographer_ros/metrics/internal/gauge.h"
-#include "cartographer_ros/metrics/internal/histogram.h"
-#include "cartographer_ros_msgs/ReadMetrics.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-// Realizes the factory / registry interface for the metrics in libcartographer
-// and provides a wrapper to collect ROS messages from the metrics it owns.
-class FamilyFactory : public ::cartographer::metrics::FamilyFactory {
- public:
-  ::cartographer::metrics::Family<::cartographer::metrics::Counter>*
-
-  NewCounterFamily(const std::string& name,
-                   const std::string& description) override;
-  ::cartographer::metrics::Family<::cartographer::metrics::Gauge>*
-  NewGaugeFamily(const std::string& name,
-                 const std::string& description) override;
-  ::cartographer::metrics::Family<::cartographer::metrics::Histogram>*
-  NewHistogramFamily(const std::string& name, const std::string& description,
-                     const ::cartographer::metrics::Histogram::BucketBoundaries&
-                         boundaries) override;
-
-  void ReadMetrics(
-      ::cartographer_ros_msgs::ReadMetrics::Response* response) const;
-
- private:
-  std::vector<std::unique_ptr<CounterFamily>> counter_families_;
-  std::vector<std::unique_ptr<GaugeFamily>> gauge_families_;
-  std::vector<std::unique_ptr<HistogramFamily>> histogram_families_;
-};
-
-}  // namespace metrics
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_METRICS_FAMILY_FACTORY_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/counter.h

@@ -1,51 +0,0 @@
-/*
- * Copyright 2018 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_METRICS_INTERNAL_COUNTER_H
-#define CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H
-
-#include "cartographer/metrics/counter.h"
-#include "cartographer_ros/metrics/internal/gauge.h"
-#include "cartographer_ros_msgs/Metric.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-class Counter : public ::cartographer::metrics::Counter {
- public:
-  explicit Counter(const std::map<std::string, std::string>& labels)
-      : gauge_(labels) {}
-
-  void Increment(const double value) override { gauge_.Increment(value); }
-
-  void Increment() override { gauge_.Increment(); }
-
-  double Value() { return gauge_.Value(); }
-
-  cartographer_ros_msgs::Metric ToRosMessage() {
-    cartographer_ros_msgs::Metric msg = gauge_.ToRosMessage();
-    msg.type = cartographer_ros_msgs::Metric::TYPE_COUNTER;
-    return msg;
-  }
-
- private:
-  Gauge gauge_;
-};
-
-}  // namespace metrics
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_COUNTER_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/family.cc

@@ -1,82 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer_ros/metrics/internal/family.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer_ros/metrics/internal/counter.h"
-#include "cartographer_ros/metrics/internal/gauge.h"
-#include "cartographer_ros/metrics/internal/histogram.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
-
-Counter* CounterFamily::Add(const std::map<std::string, std::string>& labels) {
-  auto wrapper = absl::make_unique<Counter>(labels);
-  auto* ptr = wrapper.get();
-  wrappers_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-cartographer_ros_msgs::MetricFamily CounterFamily::ToRosMessage() {
-  cartographer_ros_msgs::MetricFamily family_msg;
-  family_msg.name = name_;
-  family_msg.description = description_;
-  for (const auto& wrapper : wrappers_) {
-    family_msg.metrics.push_back(wrapper->ToRosMessage());
-  }
-  return family_msg;
-}
-
-Gauge* GaugeFamily::Add(const std::map<std::string, std::string>& labels) {
-  auto wrapper = absl::make_unique<Gauge>(labels);
-  auto* ptr = wrapper.get();
-  wrappers_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-cartographer_ros_msgs::MetricFamily GaugeFamily::ToRosMessage() {
-  cartographer_ros_msgs::MetricFamily family_msg;
-  family_msg.name = name_;
-  family_msg.description = description_;
-  for (const auto& wrapper : wrappers_) {
-    family_msg.metrics.push_back(wrapper->ToRosMessage());
-  }
-  return family_msg;
-}
-
-Histogram* HistogramFamily::Add(
-    const std::map<std::string, std::string>& labels) {
-  auto wrapper = absl::make_unique<Histogram>(labels, boundaries_);
-  auto* ptr = wrapper.get();
-  wrappers_.emplace_back(std::move(wrapper));
-  return ptr;
-}
-
-cartographer_ros_msgs::MetricFamily HistogramFamily::ToRosMessage() {
-  cartographer_ros_msgs::MetricFamily family_msg;
-  family_msg.name = name_;
-  family_msg.description = description_;
-  for (const auto& wrapper : wrappers_) {
-    family_msg.metrics.push_back(wrapper->ToRosMessage());
-  }
-  return family_msg;
-}
-
-}  // namespace metrics
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/family.h

@@ -1,81 +0,0 @@
-/*
- * Copyright 2018 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_METRICS_INTERNAL_FAMILY_H
-#define CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H
-
-#include <memory>
-#include <string>
-
-#include "cartographer/metrics/family_factory.h"
-#include "cartographer_ros/metrics/internal/counter.h"
-#include "cartographer_ros/metrics/internal/gauge.h"
-#include "cartographer_ros/metrics/internal/histogram.h"
-#include "cartographer_ros_msgs/MetricFamily.h"
-
-namespace cartographer_ros {
-namespace metrics {
-class CounterFamily
-    : public ::cartographer::metrics::Family<::cartographer::metrics::Counter> {
- public:
-  CounterFamily(const std::string& name, const std::string& description)
-      : name_(name), description_(description) {}
-  Counter* Add(const std::map<std::string, std::string>& labels) override;
-  cartographer_ros_msgs::MetricFamily ToRosMessage();
-
- private:
-  std::string name_;
-  std::string description_;
-  std::vector<std::unique_ptr<Counter>> wrappers_;
-};
-
-class GaugeFamily
-    : public ::cartographer::metrics::Family<::cartographer::metrics::Gauge> {
- public:
-  GaugeFamily(const std::string& name, const std::string& description)
-      : name_(name), description_(description) {}
-  Gauge* Add(const std::map<std::string, std::string>& labels) override;
-
-  cartographer_ros_msgs::MetricFamily ToRosMessage();
-
- private:
-  std::string name_;
-  std::string description_;
-  std::vector<std::unique_ptr<Gauge>> wrappers_;
-};
-
-class HistogramFamily : public ::cartographer::metrics::Family<
-                            ::cartographer::metrics::Histogram> {
- public:
-  HistogramFamily(const std::string& name, const std::string& description,
-                  const BucketBoundaries& boundaries)
-      : name_(name), description_(description), boundaries_(boundaries) {}
-
-  Histogram* Add(const std::map<std::string, std::string>& labels) override;
-
-  cartographer_ros_msgs::MetricFamily ToRosMessage();
-
- private:
-  std::string name_;
-  std::string description_;
-  std::vector<std::unique_ptr<Histogram>> wrappers_;
-  const BucketBoundaries boundaries_;
-};
-
-}  // namespace metrics
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_FAMILY_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/gauge.h

@@ -1,80 +0,0 @@
-/*
- * Copyright 2018 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_METRICS_INTERNAL_GAUGE_H
-#define CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H
-
-#include <map>
-#include <string>
-
-#include "absl/synchronization/mutex.h"
-#include "cartographer/metrics/gauge.h"
-#include "cartographer_ros_msgs/Metric.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-class Gauge : public ::cartographer::metrics::Gauge {
- public:
-  explicit Gauge(const std::map<std::string, std::string>& labels)
-      : labels_(labels), value_(0.) {}
-
-  void Decrement(const double value) override { Add(-1. * value); }
-
-  void Decrement() override { Decrement(1.); }
-
-  void Increment(const double value) override { Add(value); }
-
-  void Increment() override { Increment(1.); }
-
-  void Set(double value) override {
-    absl::MutexLock lock(&mutex_);
-    value_ = value;
-  }
-
-  double Value() {
-    absl::MutexLock lock(&mutex_);
-    return value_;
-  }
-
-  cartographer_ros_msgs::Metric ToRosMessage() {
-    cartographer_ros_msgs::Metric msg;
-    msg.type = cartographer_ros_msgs::Metric::TYPE_GAUGE;
-    for (const auto& label : labels_) {
-      cartographer_ros_msgs::MetricLabel label_msg;
-      label_msg.key = label.first;
-      label_msg.value = label.second;
-      msg.labels.push_back(label_msg);
-    }
-    msg.value = Value();
-    return msg;
-  }
-
- private:
-  void Add(const double value) {
-    absl::MutexLock lock(&mutex_);
-    value_ += value;
-  }
-
-  absl::Mutex mutex_;
-  const std::map<std::string, std::string> labels_;
-  double value_ GUARDED_BY(mutex_);
-};
-
-}  // namespace metrics
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_GAUGE_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/histogram.cc

@@ -1,90 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer_ros/metrics/internal/histogram.h"
-
-#include <algorithm>
-#include <numeric>
-
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
-
-Histogram::Histogram(const std::map<std::string, std::string>& labels,
-                     const BucketBoundaries& bucket_boundaries)
-    : labels_(labels),
-      bucket_boundaries_(bucket_boundaries),
-      bucket_counts_(bucket_boundaries.size() + 1) {
-  absl::MutexLock lock(&mutex_);
-  CHECK(std::is_sorted(std::begin(bucket_boundaries_),
-                       std::end(bucket_boundaries_)));
-}
-
-void Histogram::Observe(double value) {
-  auto bucket_index =
-      std::distance(bucket_boundaries_.begin(),
-                    std::upper_bound(bucket_boundaries_.begin(),
-                                     bucket_boundaries_.end(), value));
-  absl::MutexLock lock(&mutex_);
-  sum_ += value;
-  bucket_counts_[bucket_index] += 1;
-}
-
-std::map<double, double> Histogram::CountsByBucket() {
-  absl::MutexLock lock(&mutex_);
-  std::map<double, double> counts_by_bucket;
-  // Add the finite buckets.
-  for (size_t i = 0; i < bucket_boundaries_.size(); ++i) {
-    counts_by_bucket[bucket_boundaries_.at(i)] = bucket_counts_.at(i);
-  }
-  // Add the "infinite" bucket.
-  counts_by_bucket[kInfiniteBoundary] = bucket_counts_.back();
-  return counts_by_bucket;
-}
-
-double Histogram::Sum() {
-  absl::MutexLock lock(&mutex_);
-  return sum_;
-}
-
-double Histogram::CumulativeCount() {
-  absl::MutexLock lock(&mutex_);
-  return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.);
-}
-
-cartographer_ros_msgs::Metric Histogram::ToRosMessage() {
-  cartographer_ros_msgs::Metric msg;
-  msg.type = cartographer_ros_msgs::Metric::TYPE_HISTOGRAM;
-  for (const auto& label : labels_) {
-    cartographer_ros_msgs::MetricLabel label_msg;
-    label_msg.key = label.first;
-    label_msg.value = label.second;
-    msg.labels.push_back(label_msg);
-  }
-  for (const auto& bucket : CountsByBucket()) {
-    cartographer_ros_msgs::HistogramBucket bucket_msg;
-    bucket_msg.bucket_boundary = bucket.first;
-    bucket_msg.count = bucket.second;
-    msg.counts_by_bucket.push_back(bucket_msg);
-  }
-  return msg;
-}
-
-}  // namespace metrics
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/histogram.h

@@ -1,60 +0,0 @@
-/*
- * Copyright 2018 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_METRICS_INTERNAL_HISTOGRAM_H
-#define CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H
-
-#include <map>
-#include <vector>
-
-#include "absl/synchronization/mutex.h"
-#include "cartographer/metrics/histogram.h"
-#include "cartographer_ros_msgs/Metric.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-constexpr double kInfiniteBoundary = std::numeric_limits<double>::infinity();
-
-using BucketBoundaries = ::cartographer::metrics::Histogram::BucketBoundaries;
-
-class Histogram : public ::cartographer::metrics::Histogram {
- public:
-  explicit Histogram(const std::map<std::string, std::string>& labels,
-                     const BucketBoundaries& bucket_boundaries);
-
-  void Observe(double value) override;
-
-  std::map<double, double> CountsByBucket();
-
-  double Sum();
-
-  double CumulativeCount();
-
-  cartographer_ros_msgs::Metric ToRosMessage();
-
- private:
-  absl::Mutex mutex_;
-  const std::map<std::string, std::string> labels_;
-  const BucketBoundaries bucket_boundaries_;
-  std::vector<double> bucket_counts_ GUARDED_BY(mutex_);
-  double sum_ GUARDED_BY(mutex_);
-};
-
-}  // namespace metrics
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_METRICS_INTERNAL_HISTOGRAM_H

cartographer_ros/cartographer_ros/cartographer_ros/metrics/internal/metrics_test.cc

@@ -1,104 +0,0 @@
-/*
- * Copyright 2018 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 <algorithm>
-#include <array>
-#include <numeric>
-
-#include "cartographer/metrics/histogram.h"
-#include "cartographer_ros/metrics/internal/counter.h"
-#include "cartographer_ros/metrics/internal/gauge.h"
-#include "cartographer_ros/metrics/internal/histogram.h"
-#include "gtest/gtest.h"
-
-namespace cartographer_ros {
-namespace metrics {
-
-TEST(Metrics, GaugeTest) {
-  Gauge gauge({});
-  EXPECT_EQ(gauge.Value(), 0.);
-  gauge.Increment(1.2);
-  EXPECT_EQ(gauge.Value(), 1.2);
-  gauge.Increment();
-  EXPECT_EQ(gauge.Value(), 2.2);
-  gauge.Decrement(2.2);
-  EXPECT_EQ(gauge.Value(), 0.);
-  gauge.Decrement();
-  EXPECT_EQ(gauge.Value(), -1.);
-}
-
-TEST(Metrics, CounterTest) {
-  Counter counter({});
-  EXPECT_EQ(counter.Value(), 0.);
-  counter.Increment(1.2);
-  EXPECT_EQ(counter.Value(), 1.2);
-  counter.Increment(0.8);
-  EXPECT_EQ(counter.Value(), 2.);
-  counter.Increment();
-  EXPECT_EQ(counter.Value(), 3.);
-}
-
-TEST(Metrics, HistogramFixedWidthTest) {
-  auto boundaries = ::cartographer::metrics::Histogram::FixedWidth(1, 3);
-  Histogram histogram({}, boundaries);
-
-  // Observe some values that fit in finite buckets.
-  std::array<double, 3> values = {{0., 2, 2.5}};
-  for (const auto& value : values) {
-    histogram.Observe(value);
-  }
-  //     1     2     3    inf
-  //  1  |  0  |  2  |  0  |
-  EXPECT_EQ(histogram.CountsByBucket()[1], 1);
-  EXPECT_EQ(histogram.CountsByBucket()[2], 0);
-  EXPECT_EQ(histogram.CountsByBucket()[3], 2);
-  EXPECT_EQ(histogram.CumulativeCount(), values.size());
-  EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
-
-  // Values above the last bucket boundary should go to the "infinite" bucket.
-  histogram.Observe(3.5);
-  //     1     2     3    inf
-  //  1  |  0  |  2  |  1  |
-  EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
-}
-
-TEST(Metrics, HistogramScaledPowersOfTest) {
-  auto boundaries =
-      ::cartographer::metrics::Histogram::ScaledPowersOf(2, 1, 2048);
-  Histogram histogram({}, boundaries);
-
-  // Observe some values that fit in finite buckets.
-  std::array<double, 3> values = {{256, 512, 666}};
-  for (const auto& value : values) {
-    histogram.Observe(value);
-  }
-  // ... 256   512   1024  inf
-  // ...  |  1  |  2  |  0  |
-  EXPECT_EQ(histogram.CountsByBucket()[256], 0);
-  EXPECT_EQ(histogram.CountsByBucket()[512], 1);
-  EXPECT_EQ(histogram.CountsByBucket()[1024], 2);
-  EXPECT_EQ(histogram.CumulativeCount(), values.size());
-  EXPECT_EQ(histogram.Sum(), std::accumulate(values.begin(), values.end(), 0.));
-
-  // Values above the last bucket boundary should go to the "infinite" bucket.
-  histogram.Observe(2048);
-  // ... 256   512   1024  inf
-  // ...  |  1  |  2  |  1  |
-  EXPECT_EQ(histogram.CountsByBucket()[kInfiniteBoundary], 1);
-}
-
-}  // namespace metrics
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -1,420 +0,0 @@
-/*
- * 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 "cartographer_ros/msg_conversion.h"
-
-#include <cmath>
-
-#include "cartographer/common/math.h"
-#include "cartographer/common/port.h"
-#include "cartographer/common/time.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/transform/proto/transform.pb.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/time_conversion.h"
-#include "geometry_msgs/Pose.h"
-#include "geometry_msgs/Quaternion.h"
-#include "geometry_msgs/Transform.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "geometry_msgs/Vector3.h"
-#include "glog/logging.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "pcl/point_cloud.h"
-#include "pcl/point_types.h"
-#include "pcl_conversions/pcl_conversions.h"
-#include "ros/ros.h"
-#include "ros/serialization.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/PointCloud2.h"
-
-namespace {
-
-// Sizes of PCL point types have to be 4n floats for alignment, as described in
-// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php
-struct PointXYZT {
-  float x;
-  float y;
-  float z;
-  float time;
-};
-
-struct PointXYZIT {
-  PCL_ADD_POINT4D;
-  float intensity;
-  float time;
-  float unused_padding[2];
-};
-
-}  // namespace
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-    PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time))
-
-POINT_CLOUD_REGISTER_POINT_STRUCT(
-    PointXYZIT,
-    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
-                                            intensity)(float, time, time))
-
-namespace cartographer_ros {
-namespace {
-
-// The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
-// point. The last one must be this value or RViz is not showing the point cloud
-// properly.
-constexpr float kPointCloudComponentFourMagic = 1.;
-
-using ::cartographer::sensor::LandmarkData;
-using ::cartographer::sensor::LandmarkObservation;
-using ::cartographer::sensor::PointCloudWithIntensities;
-using ::cartographer::transform::Rigid3d;
-using ::cartographer_ros_msgs::LandmarkEntry;
-using ::cartographer_ros_msgs::LandmarkList;
-
-sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
-                                                   const std::string& frame_id,
-                                                   const int num_points) {
-  sensor_msgs::PointCloud2 msg;
-  msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
-  msg.header.frame_id = frame_id;
-  msg.height = 1;
-  msg.width = num_points;
-  msg.fields.resize(3);
-  msg.fields[0].name = "x";
-  msg.fields[0].offset = 0;
-  msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
-  msg.fields[0].count = 1;
-  msg.fields[1].name = "y";
-  msg.fields[1].offset = 4;
-  msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
-  msg.fields[1].count = 1;
-  msg.fields[2].name = "z";
-  msg.fields[2].offset = 8;
-  msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
-  msg.fields[2].count = 1;
-  msg.is_bigendian = false;
-  msg.point_step = 16;
-  msg.row_step = 16 * msg.width;
-  msg.is_dense = true;
-  msg.data.resize(16 * num_points);
-  return msg;
-}
-
-// For sensor_msgs::LaserScan.
-bool HasEcho(float) { return true; }
-
-float GetFirstEcho(float range) { return range; }
-
-// For sensor_msgs::MultiEchoLaserScan.
-bool HasEcho(const sensor_msgs::LaserEcho& echo) {
-  return !echo.echoes.empty();
-}
-
-float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
-  return echo.echoes[0];
-}
-
-// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
-template <typename LaserMessageType>
-std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
-LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
-  CHECK_GE(msg.range_min, 0.f);
-  CHECK_GE(msg.range_max, msg.range_min);
-  if (msg.angle_increment > 0.f) {
-    CHECK_GT(msg.angle_max, msg.angle_min);
-  } else {
-    CHECK_GT(msg.angle_min, msg.angle_max);
-  }
-  PointCloudWithIntensities point_cloud;
-  float angle = msg.angle_min;
-  for (size_t i = 0; i < msg.ranges.size(); ++i) {
-    const auto& echoes = msg.ranges[i];
-    if (HasEcho(echoes)) {
-      const float first_echo = GetFirstEcho(echoes);
-      if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
-        const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
-        const cartographer::sensor::TimedRangefinderPoint point{
-            rotation * (first_echo * Eigen::Vector3f::UnitX()),
-            i * msg.time_increment};
-        point_cloud.points.push_back(point);
-        if (msg.intensities.size() > 0) {
-          CHECK_EQ(msg.intensities.size(), msg.ranges.size());
-          const auto& echo_intensities = msg.intensities[i];
-          CHECK(HasEcho(echo_intensities));
-          point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
-        } else {
-          point_cloud.intensities.push_back(0.f);
-        }
-      }
-    }
-    angle += msg.angle_increment;
-  }
-  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
-  if (!point_cloud.points.empty()) {
-    const double duration = point_cloud.points.back().time;
-    timestamp += cartographer::common::FromSeconds(duration);
-    for (auto& point : point_cloud.points) {
-      point.time -= duration;
-    }
-  }
-  return std::make_tuple(point_cloud, timestamp);
-}
-
-bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
-                         const std::string& field_name) {
-  for (const auto& field : pc2.fields) {
-    if (field.name == field_name) {
-      return true;
-    }
-  }
-  return false;
-}
-
-}  // namespace
-
-sensor_msgs::PointCloud2 ToPointCloud2Message(
-    const int64_t timestamp, const std::string& frame_id,
-    const ::cartographer::sensor::TimedPointCloud& point_cloud) {
-  auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
-  ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
-  for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) {
-    stream.next(point.position.x());
-    stream.next(point.position.y());
-    stream.next(point.position.z());
-    stream.next(kPointCloudComponentFourMagic);
-  }
-  return msg;
-}
-
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
-  return LaserScanToPointCloudWithIntensities(msg);
-}
-
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
-  return LaserScanToPointCloudWithIntensities(msg);
-}
-
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
-  PointCloudWithIntensities point_cloud;
-  // We check for intensity field here to avoid run-time warnings if we pass in
-  // a PointCloud2 without intensity.
-  if (PointCloud2HasField(msg, "intensity")) {
-    if (PointCloud2HasField(msg, "time")) {
-      pcl::PointCloud<PointXYZIT> pcl_point_cloud;
-      pcl::fromROSMsg(msg, pcl_point_cloud);
-      point_cloud.points.reserve(pcl_point_cloud.size());
-      point_cloud.intensities.reserve(pcl_point_cloud.size());
-      for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.push_back(
-            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
-        point_cloud.intensities.push_back(point.intensity);
-      }
-    } else {
-      pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
-      pcl::fromROSMsg(msg, pcl_point_cloud);
-      point_cloud.points.reserve(pcl_point_cloud.size());
-      point_cloud.intensities.reserve(pcl_point_cloud.size());
-      for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.push_back(
-            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
-        point_cloud.intensities.push_back(point.intensity);
-      }
-    }
-  } else {
-    // If we don't have an intensity field, just copy XYZ and fill in 1.0f.
-    if (PointCloud2HasField(msg, "time")) {
-      pcl::PointCloud<PointXYZT> pcl_point_cloud;
-      pcl::fromROSMsg(msg, pcl_point_cloud);
-      point_cloud.points.reserve(pcl_point_cloud.size());
-      point_cloud.intensities.reserve(pcl_point_cloud.size());
-      for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.push_back(
-            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
-        point_cloud.intensities.push_back(1.0f);
-      }
-    } else {
-      pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
-      pcl::fromROSMsg(msg, pcl_point_cloud);
-      point_cloud.points.reserve(pcl_point_cloud.size());
-      point_cloud.intensities.reserve(pcl_point_cloud.size());
-      for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.push_back(
-            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
-        point_cloud.intensities.push_back(1.0f);
-      }
-    }
-  }
-  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
-  if (!point_cloud.points.empty()) {
-    const double duration = point_cloud.points.back().time;
-    timestamp += cartographer::common::FromSeconds(duration);
-    for (auto& point : point_cloud.points) {
-      point.time -= duration;
-      CHECK_LE(point.time, 0.f)
-          << "Encountered a point with a larger stamp than "
-             "the last point in the cloud.";
-    }
-  }
-  return std::make_tuple(point_cloud, timestamp);
-}
-
-LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
-  LandmarkData landmark_data;
-  landmark_data.time = FromRos(landmark_list.header.stamp);
-  for (const LandmarkEntry& entry : landmark_list.landmarks) {
-    landmark_data.landmark_observations.push_back(
-        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
-         entry.translation_weight, entry.rotation_weight});
-  }
-  return landmark_data;
-}
-
-Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
-  return Rigid3d(ToEigen(transform.transform.translation),
-                 ToEigen(transform.transform.rotation));
-}
-
-Rigid3d ToRigid3d(const geometry_msgs::Pose& pose) {
-  return Rigid3d({pose.position.x, pose.position.y, pose.position.z},
-                 ToEigen(pose.orientation));
-}
-
-Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3) {
-  return Eigen::Vector3d(vector3.x, vector3.y, vector3.z);
-}
-
-Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion) {
-  return Eigen::Quaterniond(quaternion.w, quaternion.x, quaternion.y,
-                            quaternion.z);
-}
-
-geometry_msgs::Transform ToGeometryMsgTransform(const Rigid3d& rigid3d) {
-  geometry_msgs::Transform transform;
-  transform.translation.x = rigid3d.translation().x();
-  transform.translation.y = rigid3d.translation().y();
-  transform.translation.z = rigid3d.translation().z();
-  transform.rotation.w = rigid3d.rotation().w();
-  transform.rotation.x = rigid3d.rotation().x();
-  transform.rotation.y = rigid3d.rotation().y();
-  transform.rotation.z = rigid3d.rotation().z();
-  return transform;
-}
-
-geometry_msgs::Pose ToGeometryMsgPose(const Rigid3d& rigid3d) {
-  geometry_msgs::Pose pose;
-  pose.position = ToGeometryMsgPoint(rigid3d.translation());
-  pose.orientation.w = rigid3d.rotation().w();
-  pose.orientation.x = rigid3d.rotation().x();
-  pose.orientation.y = rigid3d.rotation().y();
-  pose.orientation.z = rigid3d.rotation().z();
-  return pose;
-}
-
-geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d) {
-  geometry_msgs::Point point;
-  point.x = vector3d.x();
-  point.y = vector3d.y();
-  point.z = vector3d.z();
-  return point;
-}
-
-Eigen::Vector3d LatLongAltToEcef(const double latitude, const double longitude,
-                                 const double altitude) {
-  // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
-  constexpr double a = 6378137.;  // semi-major axis, equator to center.
-  constexpr double f = 1. / 298.257223563;
-  constexpr double b = a * (1. - f);  // semi-minor axis, pole to center.
-  constexpr double a_squared = a * a;
-  constexpr double b_squared = b * b;
-  constexpr double e_squared = (a_squared - b_squared) / a_squared;
-  const double sin_phi = std::sin(cartographer::common::DegToRad(latitude));
-  const double cos_phi = std::cos(cartographer::common::DegToRad(latitude));
-  const double sin_lambda = std::sin(cartographer::common::DegToRad(longitude));
-  const double cos_lambda = std::cos(cartographer::common::DegToRad(longitude));
-  const double N = a / std::sqrt(1 - e_squared * sin_phi * sin_phi);
-  const double x = (N + altitude) * cos_phi * cos_lambda;
-  const double y = (N + altitude) * cos_phi * sin_lambda;
-  const double z = (b_squared / a_squared * N + altitude) * sin_phi;
-
-  return Eigen::Vector3d(x, y, z);
-}
-
-cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(
-    const double latitude, const double longitude) {
-  const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.);
-  const Eigen::Quaterniond rotation =
-      Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.),
-                        Eigen::Vector3d::UnitY()) *
-      Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude),
-                        Eigen::Vector3d::UnitZ());
-  return cartographer::transform::Rigid3d(rotation * -translation, rotation);
-}
-
-std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
-    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
-    const double resolution, const std::string& frame_id,
-    const ros::Time& time) {
-  auto occupancy_grid = absl::make_unique<nav_msgs::OccupancyGrid>();
-
-  const int width = cairo_image_surface_get_width(painted_slices.surface.get());
-  const int height =
-      cairo_image_surface_get_height(painted_slices.surface.get());
-
-  occupancy_grid->header.stamp = time;
-  occupancy_grid->header.frame_id = frame_id;
-  occupancy_grid->info.map_load_time = time;
-  occupancy_grid->info.resolution = resolution;
-  occupancy_grid->info.width = width;
-  occupancy_grid->info.height = height;
-  occupancy_grid->info.origin.position.x =
-      -painted_slices.origin.x() * resolution;
-  occupancy_grid->info.origin.position.y =
-      (-height + painted_slices.origin.y()) * resolution;
-  occupancy_grid->info.origin.position.z = 0.;
-  occupancy_grid->info.origin.orientation.w = 1.;
-  occupancy_grid->info.origin.orientation.x = 0.;
-  occupancy_grid->info.origin.orientation.y = 0.;
-  occupancy_grid->info.origin.orientation.z = 0.;
-
-  const uint32_t* pixel_data = reinterpret_cast<uint32_t*>(
-      cairo_image_surface_get_data(painted_slices.surface.get()));
-  occupancy_grid->data.reserve(width * height);
-  for (int y = height - 1; y >= 0; --y) {
-    for (int x = 0; x < width; ++x) {
-      const uint32_t packed = pixel_data[y * width + x];
-      const unsigned char color = packed >> 16;
-      const unsigned char observed = packed >> 8;
-      const int value =
-          observed == 0
-              ? -1
-              : ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
-      CHECK_LE(-1, value);
-      CHECK_GE(100, value);
-      occupancy_grid->data.push_back(value);
-    }
-  }
-
-  return occupancy_grid;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.h

@@ -1,94 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_MSG_CONVERSION_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
-
-#include "cartographer/common/time.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/sensor/landmark_data.h"
-#include "cartographer/sensor/point_cloud.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer_ros_msgs/LandmarkList.h"
-#include "geometry_msgs/Pose.h"
-#include "geometry_msgs/Transform.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/PointCloud2.h"
-
-namespace cartographer_ros {
-
-sensor_msgs::PointCloud2 ToPointCloud2Message(
-    int64_t timestamp, const std::string& frame_id,
-    const ::cartographer::sensor::TimedPointCloud& point_cloud);
-
-geometry_msgs::Transform ToGeometryMsgTransform(
-    const ::cartographer::transform::Rigid3d& rigid3d);
-
-geometry_msgs::Pose ToGeometryMsgPose(
-    const ::cartographer::transform::Rigid3d& rigid3d);
-
-geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d);
-
-// Converts ROS message to point cloud. Returns the time when the last point
-// was acquired (different from the ROS timestamp). Timing of points is given in
-// the fourth component of each point relative to `Time`.
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);
-
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
-
-std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-           ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg);
-
-::cartographer::sensor::LandmarkData ToLandmarkData(
-    const cartographer_ros_msgs::LandmarkList& landmark_list);
-
-::cartographer::transform::Rigid3d ToRigid3d(
-    const geometry_msgs::TransformStamped& transform);
-
-::cartographer::transform::Rigid3d ToRigid3d(const geometry_msgs::Pose& pose);
-
-Eigen::Vector3d ToEigen(const geometry_msgs::Vector3& vector3);
-
-Eigen::Quaterniond ToEigen(const geometry_msgs::Quaternion& quaternion);
-
-// Converts from WGS84 (latitude, longitude, altitude) to ECEF.
-Eigen::Vector3d LatLongAltToEcef(double latitude, double longitude,
-                                 double altitude);
-
-// Returns a transform that takes ECEF coordinates from nearby points to a local
-// frame that has z pointing upwards.
-cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(double latitude,
-                                                              double longitude);
-
-// Points to an occupancy grid message at a specific resolution from painted
-// submap slices obtained via ::cartographer::io::PaintSubmapSlices(...).
-std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
-    const cartographer::io::PaintSubmapSlicesResult& painted_slices,
-    const double resolution, const std::string& frame_id,
-    const ros::Time& time);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H

cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -1,212 +0,0 @@
-/*
- * 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 <cmath>
-#include <random>
-
-#include "cartographer/transform/rigid_transform_test_helpers.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/time_conversion.h"
-#include "gmock/gmock.h"
-#include "gtest/gtest.h"
-#include "sensor_msgs/LaserScan.h"
-
-namespace cartographer_ros {
-namespace {
-
-using ::cartographer::sensor::LandmarkData;
-using ::cartographer::sensor::LandmarkObservation;
-using ::testing::AllOf;
-using ::testing::DoubleNear;
-using ::testing::ElementsAre;
-using ::testing::Field;
-
-constexpr double kEps = 1e-6;
-
-TEST(MsgConversion, LaserScanToPointCloud) {
-  sensor_msgs::LaserScan laser_scan;
-  for (int i = 0; i < 8; ++i) {
-    laser_scan.ranges.push_back(1.f);
-  }
-  laser_scan.angle_min = 0.f;
-  laser_scan.angle_max = 8.f * static_cast<float>(M_PI_4);
-  laser_scan.angle_increment = static_cast<float>(M_PI_4);
-  laser_scan.range_min = 0.f;
-  laser_scan.range_max = 10.f;
-
-  const auto point_cloud =
-      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
-  EXPECT_TRUE(
-      point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[1].position.isApprox(
-      Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
-  EXPECT_TRUE(
-      point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[3].position.isApprox(
-      Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
-  EXPECT_TRUE(
-      point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[5].position.isApprox(
-      Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
-      kEps));
-  EXPECT_TRUE(
-      point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[7].position.isApprox(
-      Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
-  for (int i = 0; i < 8; ++i) {
-    EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
-  }
-}
-
-TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
-  sensor_msgs::LaserScan laser_scan;
-  laser_scan.ranges.push_back(1.f);
-  laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
-  laser_scan.ranges.push_back(2.f);
-  laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
-  laser_scan.ranges.push_back(3.f);
-  laser_scan.angle_min = 0.f;
-  laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
-  laser_scan.angle_increment = static_cast<float>(M_PI_4);
-  laser_scan.range_min = 2.f;
-  laser_scan.range_max = 10.f;
-
-  const auto point_cloud =
-      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
-  ASSERT_EQ(2, point_cloud.size());
-  EXPECT_TRUE(
-      point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps));
-  EXPECT_TRUE(
-      point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps));
-  EXPECT_NEAR(point_cloud[0].time, 0.f, kEps);
-  EXPECT_NEAR(point_cloud[1].time, 0.f, kEps);
-}
-
-::testing::Matcher<const LandmarkObservation&> EqualsLandmark(
-    const LandmarkObservation& expected) {
-  return ::testing::AllOf(
-      Field(&LandmarkObservation::id, expected.id),
-      Field(&LandmarkObservation::landmark_to_tracking_transform,
-            ::cartographer::transform::IsNearly(
-                expected.landmark_to_tracking_transform, kEps)),
-      Field(&LandmarkObservation::translation_weight,
-            DoubleNear(expected.translation_weight, kEps)),
-      Field(&LandmarkObservation::rotation_weight,
-            DoubleNear(expected.rotation_weight, kEps)));
-}
-
-TEST(MsgConversion, LandmarkListToLandmarkData) {
-  cartographer_ros_msgs::LandmarkList message;
-  message.header.stamp.fromSec(10);
-
-  cartographer_ros_msgs::LandmarkEntry landmark_0;
-  landmark_0.id = "landmark_0";
-  landmark_0.tracking_from_landmark_transform.position.x = 1.0;
-  landmark_0.tracking_from_landmark_transform.position.y = 2.0;
-  landmark_0.tracking_from_landmark_transform.position.z = 3.0;
-  landmark_0.tracking_from_landmark_transform.orientation.w = 1.0;
-  landmark_0.tracking_from_landmark_transform.orientation.x = 0.0;
-  landmark_0.tracking_from_landmark_transform.orientation.y = 0.0;
-  landmark_0.tracking_from_landmark_transform.orientation.z = 0.0;
-  landmark_0.translation_weight = 1.0;
-  landmark_0.rotation_weight = 2.0;
-  message.landmarks.push_back(landmark_0);
-
-  LandmarkData actual_landmark_data = ToLandmarkData(message);
-  EXPECT_THAT(actual_landmark_data,
-              AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)),
-                    Field(&LandmarkData::landmark_observations,
-                          ElementsAre(EqualsLandmark(LandmarkObservation{
-                              "landmark_0",
-                              ::cartographer::transform::Rigid3d(
-                                  Eigen::Vector3d(1., 2., 3.),
-                                  Eigen::Quaterniond(1., 0., 0., 0.)),
-                              1.f,
-                              2.f,
-                          })))));
-}
-
-TEST(MsgConversion, LatLongAltToEcef) {
-  Eigen::Vector3d equator_prime_meridian = LatLongAltToEcef(0, 0, 0);
-  EXPECT_TRUE(equator_prime_meridian.isApprox(Eigen::Vector3d(6378137, 0, 0)))
-      << equator_prime_meridian;
-  Eigen::Vector3d plus_ten_meters = LatLongAltToEcef(0, 0, 10);
-  EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0)))
-      << plus_ten_meters;
-  Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0);
-  EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
-      << north_pole;
-  Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0);
-  EXPECT_TRUE(
-      also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
-      << also_north_pole;
-  Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0);
-  EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps))
-      << south_pole;
-  Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20);
-  EXPECT_TRUE(
-      above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps))
-      << above_south_pole;
-  Eigen::Vector3d somewhere_on_earth =
-      LatLongAltToEcef(48.1372149, 11.5748024, 517.1);
-  EXPECT_TRUE(somewhere_on_earth.isApprox(
-      Eigen::Vector3d(4177983, 855702, 4727457), kEps))
-      << somewhere_on_earth;
-}
-
-TEST(MsgConversion, ComputeLocalFrameFromLatLong) {
-  cartographer::transform::Rigid3d north_pole =
-      ComputeLocalFrameFromLatLong(90., 0.);
-  EXPECT_TRUE((north_pole * LatLongAltToEcef(90., 0., 1.))
-                  .isApprox(Eigen::Vector3d::UnitZ()));
-  cartographer::transform::Rigid3d south_pole =
-      ComputeLocalFrameFromLatLong(-90., 0.);
-  EXPECT_TRUE((south_pole * LatLongAltToEcef(-90., 0., 1.))
-                  .isApprox(Eigen::Vector3d::UnitZ()));
-  cartographer::transform::Rigid3d prime_meridian_equator =
-      ComputeLocalFrameFromLatLong(0., 0.);
-  EXPECT_TRUE((prime_meridian_equator * LatLongAltToEcef(0., 0., 1.))
-                  .isApprox(Eigen::Vector3d::UnitZ()))
-      << prime_meridian_equator * LatLongAltToEcef(0., 0., 1.);
-  cartographer::transform::Rigid3d meridian_90_equator =
-      ComputeLocalFrameFromLatLong(0., 90.);
-  EXPECT_TRUE((meridian_90_equator * LatLongAltToEcef(0., 90., 1.))
-                  .isApprox(Eigen::Vector3d::UnitZ()))
-      << meridian_90_equator * LatLongAltToEcef(0., 90., 1.);
-
-  std::mt19937 rng(42);
-  std::uniform_real_distribution<double> lat_distribution(-90., 90.);
-  std::uniform_real_distribution<double> long_distribution(-180., 180.);
-  std::uniform_real_distribution<double> alt_distribution(-519., 519.);
-
-  for (int i = 0; i < 1000; ++i) {
-    const double latitude = lat_distribution(rng);
-    const double longitude = long_distribution(rng);
-    const double altitude = alt_distribution(rng);
-    cartographer::transform::Rigid3d transform_to_local_frame =
-        ComputeLocalFrameFromLatLong(latitude, longitude);
-    EXPECT_TRUE((transform_to_local_frame *
-                 LatLongAltToEcef(latitude, longitude, altitude))
-                    .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps))
-        << transform_to_local_frame *
-               LatLongAltToEcef(latitude, longitude, altitude)
-        << "\n"
-        << altitude;
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node.cc

@@ -1,841 +0,0 @@
-/*
- * 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 "cartographer_ros/node.h"
-
-#include <chrono>
-#include <string>
-#include <vector>
-
-#include "Eigen/Core"
-#include "absl/memory/memory.h"
-#include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/port.h"
-#include "cartographer/common/time.h"
-#include "cartographer/mapping/pose_graph_interface.h"
-#include "cartographer/mapping/proto/submap_visualization.pb.h"
-#include "cartographer/metrics/register.h"
-#include "cartographer/sensor/point_cloud.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/metrics/family_factory.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/sensor_bridge.h"
-#include "cartographer_ros/tf_bridge.h"
-#include "cartographer_ros/time_conversion.h"
-#include "cartographer_ros_msgs/StatusCode.h"
-#include "cartographer_ros_msgs/StatusResponse.h"
-#include "glog/logging.h"
-#include "nav_msgs/Odometry.h"
-#include "ros/serialization.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "tf2_eigen/tf2_eigen.h"
-#include "visualization_msgs/MarkerArray.h"
-
-namespace cartographer_ros {
-
-namespace {
-
-cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
-  cartographer_ros_msgs::SensorTopics topics;
-  topics.laser_scan_topic = kLaserScanTopic;
-  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
-  topics.point_cloud2_topic = kPointCloud2Topic;
-  topics.imu_topic = kImuTopic;
-  topics.odometry_topic = kOdometryTopic;
-  topics.nav_sat_fix_topic = kNavSatFixTopic;
-  topics.landmark_topic = kLandmarkTopic;
-  return topics;
-}
-
-// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and
-// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
-template <typename MessageType>
-::ros::Subscriber SubscribeWithHandler(
-    void (Node::*handler)(int, const std::string&,
-                          const typename MessageType::ConstPtr&),
-    const int trajectory_id, const std::string& topic,
-    ::ros::NodeHandle* const node_handle, Node* const node) {
-  return node_handle->subscribe<MessageType>(
-      topic, kInfiniteSubscriberQueueSize,
-      boost::function<void(const typename MessageType::ConstPtr&)>(
-          [node, handler, trajectory_id,
-           topic](const typename MessageType::ConstPtr& msg) {
-            (node->*handler)(trajectory_id, topic, msg);
-          }));
-}
-
-}  // namespace
-
-namespace carto = ::cartographer;
-
-using carto::transform::Rigid3d;
-using TrajectoryState =
-    ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
-
-Node::Node(
-    const NodeOptions& node_options,
-    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-    tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
-    : node_options_(node_options),
-      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
-  absl::MutexLock lock(&mutex_);
-  if (collect_metrics) {
-    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
-    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
-  }
-  //发布的 topics
-  submap_list_publisher_ =
-      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
-          kSubmapListTopic, kLatestOnlyPublisherQueueSize);
-  trajectory_node_list_publisher_ =
-      node_handle_.advertise<::visualization_msgs::MarkerArray>(
-          kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
-  landmark_poses_list_publisher_ =
-      node_handle_.advertise<::visualization_msgs::MarkerArray>(
-          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
-  constraint_list_publisher_ =
-      node_handle_.advertise<::visualization_msgs::MarkerArray>(
-          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
-      
-  //创建service节点
-  service_servers_.push_back(node_handle_.advertiseService(
-      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
-  service_servers_.push_back(node_handle_.advertiseService(
-      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
-  service_servers_.push_back(node_handle_.advertiseService(
-      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
-  service_servers_.push_back(node_handle_.advertiseService(
-      kWriteStateServiceName, &Node::HandleWriteState, this));
-  service_servers_.push_back(node_handle_.advertiseService(
-      kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
-  service_servers_.push_back(node_handle_.advertiseService(
-      kReadMetricsServiceName, &Node::HandleReadMetrics, this));
-
-  scan_matched_point_cloud_publisher_ =
-      node_handle_.advertise<sensor_msgs::PointCloud2>(
-          kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
-
-  wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(node_options_.submap_publish_period_sec),
-      &Node::PublishSubmapList, this));
-  if (node_options_.pose_publish_period_sec > 0) {
-    publish_local_trajectory_data_timer_ = node_handle_.createTimer(
-        ::ros::Duration(node_options_.pose_publish_period_sec),
-        &Node::PublishLocalTrajectoryData, this);
-  }
-  wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
-      &Node::PublishTrajectoryNodeList, this));
-  wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
-      &Node::PublishLandmarkPosesList, this));
-  wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(kConstraintPublishPeriodSec),
-      &Node::PublishConstraintList, this));
-}
-
-Node::~Node() { FinishAllTrajectories(); }
-
-::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
-
-bool Node::HandleSubmapQuery(
-    ::cartographer_ros_msgs::SubmapQuery::Request& request,
-    ::cartographer_ros_msgs::SubmapQuery::Response& response) {
-  absl::MutexLock lock(&mutex_);
-  map_builder_bridge_.HandleSubmapQuery(request, response);
-  return true;
-}
-
-void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
-  absl::MutexLock lock(&mutex_);
-  submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
-}
-
-void Node::AddExtrapolator(const int trajectory_id,
-                           const TrajectoryOptions& options) {
-  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
-  CHECK(extrapolators_.count(trajectory_id) == 0);
-  const double gravity_time_constant =
-      node_options_.map_builder_options.use_trajectory_builder_3d()
-          ? options.trajectory_builder_options.trajectory_builder_3d_options()
-                .imu_gravity_time_constant()
-          : options.trajectory_builder_options.trajectory_builder_2d_options()
-                .imu_gravity_time_constant();
-  extrapolators_.emplace(
-      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
-      std::forward_as_tuple(
-          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
-          gravity_time_constant));
-}
-
-void Node::AddSensorSamplers(const int trajectory_id,
-                             const TrajectoryOptions& options) {
-  CHECK(sensor_samplers_.count(trajectory_id) == 0);
-  sensor_samplers_.emplace(
-      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
-      std::forward_as_tuple(
-          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
-          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
-          options.landmarks_sampling_ratio));
-}
-
-void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
-  absl::MutexLock lock(&mutex_);
-  for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
-    const auto& trajectory_data = entry.second;
-
-    auto& extrapolator = extrapolators_.at(entry.first);
-    // We only publish a point cloud if it has changed. It is not needed at high
-    // frequency, and republishing it would be computationally wasteful.
-    if (trajectory_data.local_slam_data->time !=
-        extrapolator.GetLastPoseTime()) {
-      if (scan_matched_point_cloud_publisher_.getNumSubscribers() > 0) {
-        // TODO(gaschler): Consider using other message without time
-        // information.
-        carto::sensor::TimedPointCloud point_cloud;
-        point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
-                                .returns.size());
-        for (const cartographer::sensor::RangefinderPoint point :
-             trajectory_data.local_slam_data->range_data_in_local.returns) {
-          point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
-              point, 0.f /* time */));
-        }
-        scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
-            carto::common::ToUniversal(trajectory_data.local_slam_data->time),
-            node_options_.map_frame,
-            carto::sensor::TransformTimedPointCloud(
-                point_cloud, trajectory_data.local_to_map.cast<float>())));
-      }
-      extrapolator.AddPose(trajectory_data.local_slam_data->time,
-                           trajectory_data.local_slam_data->local_pose);
-    }
-
-    geometry_msgs::TransformStamped stamped_transform;
-    // If we do not publish a new point cloud, we still allow time of the
-    // published poses to advance. If we already know a newer pose, we use its
-    // time instead. Since tf knows how to interpolate, providing newer
-    // information is better.
-    const ::cartographer::common::Time now = std::max(
-        FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
-    stamped_transform.header.stamp =
-        node_options_.use_pose_extrapolator
-            ? ToRos(now)
-            : ToRos(trajectory_data.local_slam_data->time);
-    const Rigid3d tracking_to_local_3d =
-        node_options_.use_pose_extrapolator
-            ? extrapolator.ExtrapolatePose(now)
-            : trajectory_data.local_slam_data->local_pose;
-    const Rigid3d tracking_to_local = [&] {
-      if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
-        return carto::transform::Embed3D(
-            carto::transform::Project2D(tracking_to_local_3d));
-      }
-      return tracking_to_local_3d;
-    }();
-
-    const Rigid3d tracking_to_map =
-        trajectory_data.local_to_map * tracking_to_local;
-
-    if (trajectory_data.published_to_tracking != nullptr) {
-      if (trajectory_data.trajectory_options.provide_odom_frame) {
-        std::vector<geometry_msgs::TransformStamped> stamped_transforms;
-
-        stamped_transform.header.frame_id = node_options_.map_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.odom_frame;
-        stamped_transform.transform =
-            ToGeometryMsgTransform(trajectory_data.local_to_map);
-        stamped_transforms.push_back(stamped_transform);
-
-        stamped_transform.header.frame_id =
-            trajectory_data.trajectory_options.odom_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.published_frame;
-        stamped_transform.transform = ToGeometryMsgTransform(
-            tracking_to_local * (*trajectory_data.published_to_tracking));
-        stamped_transforms.push_back(stamped_transform);
-
-        tf_broadcaster_.sendTransform(stamped_transforms);
-      } else {
-        stamped_transform.header.frame_id = node_options_.map_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.published_frame;
-        stamped_transform.transform = ToGeometryMsgTransform(
-            tracking_to_map * (*trajectory_data.published_to_tracking));
-        tf_broadcaster_.sendTransform(stamped_transform);
-      }
-    }
-  }
-}
-
-void Node::PublishTrajectoryNodeList(
-    const ::ros::WallTimerEvent& unused_timer_event) {
-  if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
-    absl::MutexLock lock(&mutex_);
-    trajectory_node_list_publisher_.publish(
-        map_builder_bridge_.GetTrajectoryNodeList());
-  }
-}
-
-void Node::PublishLandmarkPosesList(
-    const ::ros::WallTimerEvent& unused_timer_event) {
-  if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
-    absl::MutexLock lock(&mutex_);
-    landmark_poses_list_publisher_.publish(
-        map_builder_bridge_.GetLandmarkPosesList());
-  }
-}
-
-void Node::PublishConstraintList(
-    const ::ros::WallTimerEvent& unused_timer_event) {
-  if (constraint_list_publisher_.getNumSubscribers() > 0) {
-    absl::MutexLock lock(&mutex_);
-    constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
-  }
-}
-
-std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-Node::ComputeExpectedSensorIds(
-    const TrajectoryOptions& options,
-    const cartographer_ros_msgs::SensorTopics& topics) const {
-  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
-  using SensorType = SensorId::SensorType;
-  std::set<SensorId> expected_topics;
-  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
-  for (const std::string& topic : ComputeRepeatedTopicNames(
-           topics.laser_scan_topic, options.num_laser_scans)) {
-    expected_topics.insert(SensorId{SensorType::RANGE, topic});
-  }
-  for (const std::string& topic :
-       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
-                                 options.num_multi_echo_laser_scans)) {
-    expected_topics.insert(SensorId{SensorType::RANGE, topic});
-  }
-  for (const std::string& topic : ComputeRepeatedTopicNames(
-           topics.point_cloud2_topic, options.num_point_clouds)) {
-    expected_topics.insert(SensorId{SensorType::RANGE, topic});
-  }
-  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  // required.
-  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
-      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
-       options.trajectory_builder_options.trajectory_builder_2d_options()
-           .use_imu_data())) {
-    expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
-  }
-  // Odometry is optional.
-  if (options.use_odometry) {
-    expected_topics.insert(
-        SensorId{SensorType::ODOMETRY, topics.odometry_topic});
-  }
-  // NavSatFix is optional.
-  if (options.use_nav_sat) {
-    expected_topics.insert(
-        SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic});
-  }
-  // Landmark is optional.
-  if (options.use_landmarks) {
-    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
-  }
-  return expected_topics;
-}
-
-int Node::AddTrajectory(const TrajectoryOptions& options,
-                        const cartographer_ros_msgs::SensorTopics& topics) 
-{
-  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
-  const int trajectory_id =
-      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
-  AddExtrapolator(trajectory_id, options);
-  AddSensorSamplers(trajectory_id, options);
-  LaunchSubscribers(options, topics, trajectory_id);
-  wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
-      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
-  for (const auto& sensor_id : expected_sensor_ids) {
-    subscribed_topics_.insert(sensor_id.id);
-  }
-  return trajectory_id;
-}
-
-void Node::LaunchSubscribers(const TrajectoryOptions& options,
-                             const cartographer_ros_msgs::SensorTopics& topics,
-                             const int trajectory_id)
-{
-  for (const std::string& topic : ComputeRepeatedTopicNames(topics.laser_scan_topic, options.num_laser_scans))
-  {
-    subscribers_[trajectory_id].push_back(
-        {
-	    SubscribeWithHandler<sensor_msgs::LaserScan>(
-             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
-             this),
-         topic}
-	);
-  }
-  for (const std::string& topic :
-       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
-                                 options.num_multi_echo_laser_scans)) {
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
-             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
-             &node_handle_, this),
-         topic});
-  }
-  for (const std::string& topic : ComputeRepeatedTopicNames(
-           topics.point_cloud2_topic, options.num_point_clouds)) {
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
-             &Node::HandlePointCloud2Message, trajectory_id, topic,
-             &node_handle_, this),
-         topic});
-  }
-
-  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  // required.
-  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
-      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
-       options.trajectory_builder_options.trajectory_builder_2d_options()
-           .use_imu_data())) {
-    std::string topic = topics.imu_topic;
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
-                                                trajectory_id, topic,
-                                                &node_handle_, this),
-         topic});
-  }
-
-  if (options.use_odometry) {
-    std::string topic = topics.odometry_topic;
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
-                                                  trajectory_id, topic,
-                                                  &node_handle_, this),
-         topic});
-  }
-  if (options.use_nav_sat) {
-    std::string topic = topics.nav_sat_fix_topic;
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
-             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
-             this),
-         topic});
-  }
-  if (options.use_landmarks) {
-    std::string topic = topics.landmark_topic;
-    subscribers_[trajectory_id].push_back(
-        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
-             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
-             this),
-         topic});
-  }
-}
-
-bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
-  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
-    return options.trajectory_builder_options
-        .has_trajectory_builder_2d_options();
-  }
-  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
-    return options.trajectory_builder_options
-        .has_trajectory_builder_3d_options();
-  }
-  return false;
-}
-
-bool Node::ValidateTopicNames(
-    const ::cartographer_ros_msgs::SensorTopics& topics,
-    const TrajectoryOptions& options) {
-  for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) {
-    const std::string& topic = sensor_id.id;
-    if (subscribed_topics_.count(topic) > 0) {
-      LOG(ERROR) << "Topic name [" << topic << "] is already used.";
-      return false;
-    }
-  }
-  return true;
-}
-
-cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
-    const int trajectory_id) {
-  auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
-
-  cartographer_ros_msgs::StatusResponse status_response;
-  if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
-    const std::string message = "Trajectory " + std::to_string(trajectory_id) +
-                                " already pending to finish.";
-    status_response.code = cartographer_ros_msgs::StatusCode::OK;
-    status_response.message = message;
-    LOG(INFO) << message;
-    return status_response;
-  }
-
-  // First, check if we can actually finish the trajectory.
-  if (!(trajectory_states.count(trajectory_id))) {
-    const std::string error =
-        "Trajectory " + std::to_string(trajectory_id) + " doesn't exist.";
-    LOG(ERROR) << error;
-    status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) {
-    const std::string error =
-        "Trajectory " + std::to_string(trajectory_id) + " is frozen.";
-    LOG(ERROR) << error;
-    status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) {
-    const std::string error = "Trajectory " + std::to_string(trajectory_id) +
-                              " has already been finished.";
-    LOG(ERROR) << error;
-    status_response.code =
-        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
-    status_response.message = error;
-    return status_response;
-  } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) {
-    const std::string error =
-        "Trajectory " + std::to_string(trajectory_id) + " has been deleted.";
-    LOG(ERROR) << error;
-    status_response.code =
-        cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
-    status_response.message = error;
-    return status_response;
-  }
-
-  // Shutdown the subscribers of this trajectory.
-  // A valid case with no subscribers is e.g. if we just visualize states.
-  if (subscribers_.count(trajectory_id)) {
-    for (auto& entry : subscribers_[trajectory_id]) {
-      entry.subscriber.shutdown();
-      subscribed_topics_.erase(entry.topic);
-      LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
-    }
-    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
-  }
-  map_builder_bridge_.FinishTrajectory(trajectory_id);
-  trajectories_scheduled_for_finish_.emplace(trajectory_id);
-  const std::string message =
-      "Finished trajectory " + std::to_string(trajectory_id) + ".";
-  status_response.code = cartographer_ros_msgs::StatusCode::OK;
-  status_response.message = message;
-  return status_response;
-}
-
-bool Node::HandleStartTrajectory(
-    ::cartographer_ros_msgs::StartTrajectory::Request& request,
-    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
-  absl::MutexLock lock(&mutex_);
-  TrajectoryOptions options;
-  if (!FromRosMessage(request.options, &options) ||
-      !ValidateTrajectoryOptions(options)) {
-    const std::string error = "Invalid trajectory options.";
-    LOG(ERROR) << error;
-    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-    response.status.message = error;
-  } else if (!ValidateTopicNames(request.topics, options)) {
-    const std::string error = "Invalid topics.";
-    LOG(ERROR) << error;
-    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-    response.status.message = error;
-  } else {
-    response.trajectory_id = AddTrajectory(options, request.topics);
-    response.status.code = cartographer_ros_msgs::StatusCode::OK;
-    response.status.message = "Success.";
-  }
-  return true;
-}
-
-void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
-  absl::MutexLock lock(&mutex_);
-  CHECK(ValidateTrajectoryOptions(options));
-  AddTrajectory(options, DefaultSensorTopics());
-}
-
-std::vector<
-    std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
-Node::ComputeDefaultSensorIdsForMultipleBags(
-    const std::vector<TrajectoryOptions>& bags_options) const {
-  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
-  std::vector<std::set<SensorId>> bags_sensor_ids;
-  for (size_t i = 0; i < bags_options.size(); ++i) {
-    std::string prefix;
-    if (bags_options.size() > 1) {
-      prefix = "bag_" + std::to_string(i + 1) + "_";
-    }
-    std::set<SensorId> unique_sensor_ids;
-    for (const auto& sensor_id :
-         ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) {
-      unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
-    }
-    bags_sensor_ids.push_back(unique_sensor_ids);
-  }
-  return bags_sensor_ids;
-}
-
-int Node::AddOfflineTrajectory(
-    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
-        expected_sensor_ids,
-    const TrajectoryOptions& options) {
-  absl::MutexLock lock(&mutex_);
-  const int trajectory_id =
-      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
-  AddExtrapolator(trajectory_id, options);
-  AddSensorSamplers(trajectory_id, options);
-  return trajectory_id;
-}
-
-bool Node::HandleGetTrajectoryStates(
-    ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
-    ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
-  using TrajectoryState =
-      ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
-  absl::MutexLock lock(&mutex_);
-  response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
-  response.trajectory_states.header.stamp = ros::Time::now();
-  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
-    response.trajectory_states.trajectory_id.push_back(entry.first);
-    switch (entry.second) {
-      case TrajectoryState::ACTIVE:
-        response.trajectory_states.trajectory_state.push_back(
-            ::cartographer_ros_msgs::TrajectoryStates::ACTIVE);
-        break;
-      case TrajectoryState::FINISHED:
-        response.trajectory_states.trajectory_state.push_back(
-            ::cartographer_ros_msgs::TrajectoryStates::FINISHED);
-        break;
-      case TrajectoryState::FROZEN:
-        response.trajectory_states.trajectory_state.push_back(
-            ::cartographer_ros_msgs::TrajectoryStates::FROZEN);
-        break;
-      case TrajectoryState::DELETED:
-        response.trajectory_states.trajectory_state.push_back(
-            ::cartographer_ros_msgs::TrajectoryStates::DELETED);
-        break;
-    }
-  }
-  return true;
-}
-
-bool Node::HandleFinishTrajectory(
-    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
-    ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
-  absl::MutexLock lock(&mutex_);
-  response.status = FinishTrajectoryUnderLock(request.trajectory_id);
-  return true;
-}
-
-bool Node::HandleWriteState(
-    ::cartographer_ros_msgs::WriteState::Request& request,
-    ::cartographer_ros_msgs::WriteState::Response& response) {
-  absl::MutexLock lock(&mutex_);
-  if (map_builder_bridge_.SerializeState(request.filename,
-                                         request.include_unfinished_submaps)) {
-    response.status.code = cartographer_ros_msgs::StatusCode::OK;
-    response.status.message = "State written to '" + request.filename + "'.";
-  } else {
-    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
-    response.status.message = "Failed to write '" + request.filename + "'.";
-  }
-  return true;
-}
-
-bool Node::HandleReadMetrics(
-    ::cartographer_ros_msgs::ReadMetrics::Request& request,
-    ::cartographer_ros_msgs::ReadMetrics::Response& response) {
-  absl::MutexLock lock(&mutex_);
-  response.timestamp = ros::Time::now();
-  if (!metrics_registry_) {
-    response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
-    response.status.message = "Collection of runtime metrics is not activated.";
-    return true;
-  }
-  metrics_registry_->ReadMetrics(&response);
-  response.status.code = cartographer_ros_msgs::StatusCode::OK;
-  response.status.message = "Successfully read metrics.";
-  return true;
-}
-
-void Node::FinishAllTrajectories() {
-  absl::MutexLock lock(&mutex_);
-  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
-    if (entry.second == TrajectoryState::ACTIVE) {
-      const int trajectory_id = entry.first;
-      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
-               cartographer_ros_msgs::StatusCode::OK);
-    }
-  }
-}
-
-bool Node::FinishTrajectory(const int trajectory_id) {
-  absl::MutexLock lock(&mutex_);
-  return FinishTrajectoryUnderLock(trajectory_id).code ==
-         cartographer_ros_msgs::StatusCode::OK;
-}
-
-void Node::RunFinalOptimization() {
-  {
-    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
-      const int trajectory_id = entry.first;
-      if (entry.second == TrajectoryState::ACTIVE) {
-        LOG(WARNING)
-            << "Can't run final optimization if there are one or more active "
-               "trajectories. Trying to finish trajectory with ID "
-            << std::to_string(trajectory_id) << " now.";
-        CHECK(FinishTrajectory(trajectory_id))
-            << "Failed to finish trajectory with ID "
-            << std::to_string(trajectory_id) << ".";
-      }
-    }
-  }
-  // Assuming we are not adding new data anymore, the final optimization
-  // can be performed without holding the mutex.
-  map_builder_bridge_.RunFinalOptimization();
-}
-
-void Node::HandleOdometryMessage(const int trajectory_id,
-                                 const std::string& sensor_id,
-                                 const nav_msgs::Odometry::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
-    return;
-  }
-  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
-  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
-  if (odometry_data_ptr != nullptr) {
-    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
-  }
-  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
-}
-
-void Node::HandleNavSatFixMessage(const int trajectory_id,
-                                  const std::string& sensor_id,
-                                  const sensor_msgs::NavSatFix::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
-    return;
-  }
-  map_builder_bridge_.sensor_bridge(trajectory_id)
-      ->HandleNavSatFixMessage(sensor_id, msg);
-}
-
-void Node::HandleLandmarkMessage(
-    const int trajectory_id, const std::string& sensor_id,
-    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
-    return;
-  }
-  map_builder_bridge_.sensor_bridge(trajectory_id)
-      ->HandleLandmarkMessage(sensor_id, msg);
-}
-
-void Node::HandleImuMessage(const int trajectory_id,
-                            const std::string& sensor_id,
-                            const sensor_msgs::Imu::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
-    return;
-  }
-  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
-  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
-  if (imu_data_ptr != nullptr) {
-    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
-  }
-  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
-}
-
-void Node::HandleLaserScanMessage(const int trajectory_id,
-                                  const std::string& sensor_id,
-                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
-    return;
-  }
-  map_builder_bridge_.sensor_bridge(trajectory_id)
-      ->HandleLaserScanMessage(sensor_id, msg);
-}
-
-void Node::HandleMultiEchoLaserScanMessage(
-    const int trajectory_id, const std::string& sensor_id,
-    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
-    return;
-  }
-  map_builder_bridge_.sensor_bridge(trajectory_id)
-      ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
-}
-
-void Node::HandlePointCloud2Message(
-    const int trajectory_id, const std::string& sensor_id,
-    const sensor_msgs::PointCloud2::ConstPtr& msg) {
-  absl::MutexLock lock(&mutex_);
-  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
-    return;
-  }
-  map_builder_bridge_.sensor_bridge(trajectory_id)
-      ->HandlePointCloud2Message(sensor_id, msg);
-}
-
-void Node::SerializeState(const std::string& filename,
-                          const bool include_unfinished_submaps) {
-  absl::MutexLock lock(&mutex_);
-  CHECK(
-      map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
-      << "Could not write state.";
-}
-
-void Node::LoadState(const std::string& state_filename,
-                     const bool load_frozen_state) {
-  absl::MutexLock lock(&mutex_);
-  map_builder_bridge_.LoadState(state_filename, load_frozen_state);
-}
-
-void Node::MaybeWarnAboutTopicMismatch(
-    const ::ros::WallTimerEvent& unused_timer_event) {
-  ::ros::master::V_TopicInfo ros_topics;
-  ::ros::master::getTopics(ros_topics);
-  std::set<std::string> published_topics;
-  std::stringstream published_topics_string;
-  for (const auto& it : ros_topics) {
-    std::string resolved_topic = node_handle_.resolveName(it.name, false);
-    published_topics.insert(resolved_topic);
-    published_topics_string << resolved_topic << ",";
-  }
-  bool print_topics = false;
-  for (const auto& entry : subscribers_) {
-    int trajectory_id = entry.first;
-    for (const auto& subscriber : entry.second) {
-      std::string resolved_topic = node_handle_.resolveName(subscriber.topic);
-      if (published_topics.count(resolved_topic) == 0) {
-        LOG(WARNING) << "Expected topic \"" << subscriber.topic
-                     << "\" (trajectory " << trajectory_id << ")"
-                     << " (resolved topic \"" << resolved_topic << "\")"
-                     << " but no publisher is currently active.";
-        print_topics = true;
-      }
-    }
-  }
-  if (print_topics) {
-    LOG(WARNING) << "Currently available topics are: "
-                 << published_topics_string.str();
-  }
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node.h

@@ -1,234 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_NODE_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H
-
-#include <map>
-#include <memory>
-#include <set>
-#include <unordered_map>
-#include <unordered_set>
-#include <vector>
-
-#include "absl/synchronization/mutex.h"
-#include "cartographer/common/fixed_ratio_sampler.h"
-#include "cartographer/mapping/map_builder_interface.h"
-#include "cartographer/mapping/pose_extrapolator.h"
-#include "cartographer_ros/map_builder_bridge.h"
-#include "cartographer_ros/metrics/family_factory.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"
-#include "cartographer_ros_msgs/GetTrajectoryStates.h"
-#include "cartographer_ros_msgs/ReadMetrics.h"
-#include "cartographer_ros_msgs/SensorTopics.h"
-#include "cartographer_ros_msgs/StartTrajectory.h"
-#include "cartographer_ros_msgs/StatusResponse.h"
-#include "cartographer_ros_msgs/SubmapEntry.h"
-#include "cartographer_ros_msgs/SubmapList.h"
-#include "cartographer_ros_msgs/SubmapQuery.h"
-#include "cartographer_ros_msgs/TrajectoryOptions.h"
-#include "cartographer_ros_msgs/WriteState.h"
-#include "nav_msgs/Odometry.h"
-#include "ros/ros.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/NavSatFix.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "tf2_ros/transform_broadcaster.h"
-
-namespace cartographer_ros {
-
-// Wires up ROS topics to SLAM.
-class Node {
- public:
-  Node(const NodeOptions& node_options,
-       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
-       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
-  ~Node();
-
-  Node(const Node&) = delete;
-  Node& operator=(const Node&) = delete;
-
-  // Finishes all yet active trajectories.
-  void FinishAllTrajectories();
-  // Finishes a single given trajectory. Returns false if the trajectory did not
-  // exist or was already finished.
-  bool FinishTrajectory(int trajectory_id);
-
-  // Runs final optimization. All trajectories have to be finished when calling.
-  void RunFinalOptimization();
-
-  // Starts the first trajectory with the default topics.
-  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
-
-  // Returns unique SensorIds for multiple input bag files based on
-  // their TrajectoryOptions.
-  // 'SensorId::id' is the expected ROS topic name.
-  std::vector<
-      std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
-  ComputeDefaultSensorIdsForMultipleBags(
-      const std::vector<TrajectoryOptions>& bags_options) const;
-
-  // Adds a trajectory for offline processing, i.e. not listening to topics.
-  int AddOfflineTrajectory(
-      const std::set<
-          cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
-          expected_sensor_ids,
-      const TrajectoryOptions& options);
-
-  // The following functions handle adding sensor data to a trajectory.
-  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
-                             const nav_msgs::Odometry::ConstPtr& msg);
-  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
-                              const sensor_msgs::NavSatFix::ConstPtr& msg);
-  void HandleLandmarkMessage(
-      int trajectory_id, const std::string& sensor_id,
-      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
-  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
-                        const sensor_msgs::Imu::ConstPtr& msg);
-  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
-                              const sensor_msgs::LaserScan::ConstPtr& msg);
-  void HandleMultiEchoLaserScanMessage(
-      int trajectory_id, const std::string& sensor_id,
-      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
-  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
-                                const sensor_msgs::PointCloud2::ConstPtr& msg);
-
-  // Serializes the complete Node state.
-  void SerializeState(const std::string& filename,
-                      const bool include_unfinished_submaps);
-
-  // Loads a serialized SLAM state from a .pbstream file.
-  void LoadState(const std::string& state_filename, bool load_frozen_state);
-
-  ::ros::NodeHandle* node_handle();
-
- private:
-  struct Subscriber {
-    ::ros::Subscriber subscriber;
-
-    // ::ros::Subscriber::getTopic() does not necessarily return the same
-    // std::string
-    // it was given in its constructor. Since we rely on the topic name as the
-    // unique identifier of a subscriber, we remember it ourselves.
-    std::string topic;
-  };
-
-  bool HandleSubmapQuery(
-      cartographer_ros_msgs::SubmapQuery::Request& request,
-      cartographer_ros_msgs::SubmapQuery::Response& response);
-  bool HandleStartTrajectory(
-      cartographer_ros_msgs::StartTrajectory::Request& request,
-      cartographer_ros_msgs::StartTrajectory::Response& response);
-  bool HandleFinishTrajectory(
-      cartographer_ros_msgs::FinishTrajectory::Request& request,
-      cartographer_ros_msgs::FinishTrajectory::Response& response);
-  bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
-                        cartographer_ros_msgs::WriteState::Response& response);
-  bool HandleGetTrajectoryStates(
-      ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
-      ::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
-  bool HandleReadMetrics(
-      cartographer_ros_msgs::ReadMetrics::Request& request,
-      cartographer_ros_msgs::ReadMetrics::Response& response);
-
-  // Returns the set of SensorIds expected for a trajectory.
-  // 'SensorId::id' is the expected ROS topic name.
-  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-  ComputeExpectedSensorIds(
-      const TrajectoryOptions& options,
-      const cartographer_ros_msgs::SensorTopics& topics) const;
-  int AddTrajectory(const TrajectoryOptions& options,
-                    const cartographer_ros_msgs::SensorTopics& topics);
-  void LaunchSubscribers(const TrajectoryOptions& options,
-                         const cartographer_ros_msgs::SensorTopics& topics,
-                         int trajectory_id);
-  void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
-  void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
-  void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
-  void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
-  void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
-  void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
-  void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
-  bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
-  bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
-                          const TrajectoryOptions& options);
-  cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
-      int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
-  void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
-
-  const NodeOptions node_options_;
-
-  tf2_ros::TransformBroadcaster tf_broadcaster_;
-
-  absl::Mutex mutex_;
-  MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
-
-  ::ros::NodeHandle node_handle_;
-  ::ros::Publisher submap_list_publisher_;
-  ::ros::Publisher trajectory_node_list_publisher_;
-  ::ros::Publisher landmark_poses_list_publisher_;
-  ::ros::Publisher constraint_list_publisher_;
-  // These ros::ServiceServers need to live for the lifetime of the node.
-  std::vector<::ros::ServiceServer> service_servers_;
-  ::ros::Publisher scan_matched_point_cloud_publisher_;
-
-  struct TrajectorySensorSamplers {
-    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
-                             const double odometry_sampling_ratio,
-                             const double fixed_frame_pose_sampling_ratio,
-                             const double imu_sampling_ratio,
-                             const double landmark_sampling_ratio)
-        : rangefinder_sampler(rangefinder_sampling_ratio),
-          odometry_sampler(odometry_sampling_ratio),
-          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
-          imu_sampler(imu_sampling_ratio),
-          landmark_sampler(landmark_sampling_ratio) {}
-
-    ::cartographer::common::FixedRatioSampler rangefinder_sampler;
-    ::cartographer::common::FixedRatioSampler odometry_sampler;
-    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
-    ::cartographer::common::FixedRatioSampler imu_sampler;
-    ::cartographer::common::FixedRatioSampler landmark_sampler;
-  };
-
-  // These are keyed with 'trajectory_id'.
-  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
-  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
-  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
-  std::unordered_set<std::string> subscribed_topics_;
-  std::unordered_set<int> trajectories_scheduled_for_finish_;
-
-  // We have to keep the timer handles of ::ros::WallTimers around, otherwise
-  // they do not fire.
-  std::vector<::ros::WallTimer> wall_timers_;
-
-  // The timer for publishing local trajectory data (i.e. pose transforms and
-  // range data point clouds) is a regular timer which is not triggered when
-  // simulation time is standing still. This prevents overflowing the transform
-  // listener buffer by publishing the same transforms over and over again.
-  ::ros::Timer publish_local_trajectory_data_timer_;
-
-  std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_H

cartographer_ros/cartographer_ros/cartographer_ros/node_constants.cc

@@ -1,37 +0,0 @@
-/*
- * 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 "cartographer_ros/node_constants.h"
-
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
-                                                   const int num_topics) {
-  CHECK_GE(num_topics, 0);
-  if (num_topics == 1) {
-    return {topic};
-  }
-  std::vector<std::string> topics;
-  topics.reserve(num_topics);
-  for (int i = 0; i < num_topics; ++i) {
-    topics.emplace_back(topic + "_" + std::to_string(i + 1));
-  }
-  return topics;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node_constants.h

@@ -1,57 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H
-
-#include <string>
-#include <vector>
-
-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 kNavSatFixTopic[] = "fix";
-constexpr char kLandmarkTopic[] = "landmark";
-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 kWriteStateServiceName[] = "write_state";
-constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states";
-constexpr char kReadMetricsServiceName[] = "read_metrics";
-constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";
-constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";
-constexpr char kConstraintListTopic[] = "constraint_list";
-constexpr double kConstraintPublishPeriodSec = 0.5;
-constexpr double kTopicMismatchCheckDelaySec = 3.0;
-
-constexpr int kInfiniteSubscriberQueueSize = 0;
-constexpr int kLatestOnlyPublisherQueueSize = 1;
-
-// For multiple topics adds numbers to the topic name and returns the list.
-std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
-                                                   int num_topics);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_CONSTANTS_H

cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc

@@ -1,100 +0,0 @@
-/*
- * 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 "absl/memory/memory.h"
-#include "cartographer/mapping/map_builder.h"
-#include "cartographer_ros/node.h"
-#include "cartographer_ros/node_options.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "gflags/gflags.h"
-#include "tf2_ros/transform_listener.h"
-
-DEFINE_bool(collect_metrics, false,
-            "Activates the collection of runtime metrics. If activated, the "
-            "metrics can be accessed via a ROS service.");
-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.");
-DEFINE_string(load_state_filename, "",
-              "If non-empty, filename of a .pbstream file to load, containing "
-              "a saved SLAM state.");
-DEFINE_bool(load_frozen_state, true,
-            "Load the saved state as frozen (non-optimized) trajectories.");
-DEFINE_bool(
-    start_trajectory_with_default_topics, true,
-    "Enable to immediately start the first trajectory with default topics.");
-DEFINE_string(
-    save_state_filename, "",
-    "If non-empty, serialize state and write it to disk before shutting down.");
-
-namespace cartographer_ros {
-namespace {
-
-void Run() {
-  constexpr double kTfBufferCacheTimeInSeconds = 10.;
-  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
-  tf2_ros::TransformListener tf(tf_buffer);
-  NodeOptions node_options;
-  TrajectoryOptions trajectory_options;
-  std::tie(node_options, trajectory_options) =
-      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
-
-  auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
-      node_options.map_builder_options);
-  Node node(node_options, std::move(map_builder), &tf_buffer,
-            FLAGS_collect_metrics);
-  if (!FLAGS_load_state_filename.empty()) {
-    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
-  }
-
-  if (FLAGS_start_trajectory_with_default_topics) {
-    node.StartTrajectoryWithDefaultTopics(trajectory_options);
-  }
-
-  ::ros::spin();
-
-  node.FinishAllTrajectories();
-  node.RunFinalOptimization();
-
-  if (!FLAGS_save_state_filename.empty()) {
-    node.SerializeState(FLAGS_save_state_filename,
-                        true /* include_unfinished_submaps */);
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  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_node");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-  cartographer_ros::Run();
-  ::ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/node_options.cc

@@ -1,65 +0,0 @@
-/*
- * 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 "cartographer_ros/node_options.h"
-
-#include <vector>
-
-#include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/mapping/map_builder.h"
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-NodeOptions CreateNodeOptions(
-    ::cartographer::common::LuaParameterDictionary* const
-        lua_parameter_dictionary) {
-  NodeOptions options;
-  options.map_builder_options =
-      ::cartographer::mapping::CreateMapBuilderOptions(
-          lua_parameter_dictionary->GetDictionary("map_builder").get());
-  options.map_frame = lua_parameter_dictionary->GetString("map_frame");
-  options.lookup_transform_timeout_sec =
-      lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
-  options.submap_publish_period_sec =
-      lua_parameter_dictionary->GetDouble("submap_publish_period_sec");
-  options.pose_publish_period_sec =
-      lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
-  options.trajectory_publish_period_sec =
-      lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
-  if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) {
-    options.use_pose_extrapolator =
-        lua_parameter_dictionary->GetBool("use_pose_extrapolator");
-  }
-  return options;
-}
-
-std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
-    const std::string& configuration_directory,
-    const std::string& configuration_basename) {
-  auto file_resolver =
-      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
-          std::vector<std::string>{configuration_directory});
-  const std::string code =
-      file_resolver->GetFileContentOrDie(configuration_basename);
-  cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
-      code, std::move(file_resolver));
-
-  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
-                         CreateTrajectoryOptions(&lua_parameter_dictionary));
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/node_options.h

@@ -1,50 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_NODE_OPTIONS_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H
-
-#include <string>
-#include <tuple>
-
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/port.h"
-#include "cartographer/mapping/proto/map_builder_options.pb.h"
-#include "cartographer_ros/trajectory_options.h"
-
-namespace cartographer_ros {
-
-// Top-level options of Cartographer's ROS integration.
-struct NodeOptions {
-  ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
-  std::string map_frame;
-  double lookup_transform_timeout_sec;
-  double submap_publish_period_sec;
-  double pose_publish_period_sec;
-  double trajectory_publish_period_sec;
-  bool use_pose_extrapolator = true;
-};
-
-NodeOptions CreateNodeOptions(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
-
-std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
-    const std::string& configuration_directory,
-    const std::string& configuration_basename);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_NODE_OPTIONS_H

cartographer_ros/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc

@@ -1,192 +0,0 @@
-/*
- * 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 <cmath>
-#include <string>
-#include <vector>
-
-#include "Eigen/Core"
-#include "Eigen/Geometry"
-#include "absl/synchronization/mutex.h"
-#include "cairo/cairo.h"
-#include "cartographer/common/port.h"
-#include "cartographer/io/image.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/mapping/id.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/node_constants.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "cartographer_ros/submap.h"
-#include "cartographer_ros_msgs/SubmapList.h"
-#include "cartographer_ros_msgs/SubmapQuery.h"
-#include "gflags/gflags.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "ros/ros.h"
-
-DEFINE_double(resolution, 0.05,
-              "Resolution of a grid cell in the published occupancy grid.");
-DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
-DEFINE_bool(include_frozen_submaps, true,
-            "Include frozen submaps in the occupancy grid.");
-DEFINE_bool(include_unfrozen_submaps, true,
-            "Include unfrozen submaps in the occupancy grid.");
-DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic,
-              "Name of the topic on which the occupancy grid is published.");
-
-namespace cartographer_ros {
-namespace {
-
-using ::cartographer::io::PaintSubmapSlicesResult;
-using ::cartographer::io::SubmapSlice;
-using ::cartographer::mapping::SubmapId;
-
-class Node {
- public:
-  explicit Node(double resolution, double publish_period_sec);
-  ~Node() {}
-
-  Node(const Node&) = delete;
-  Node& operator=(const Node&) = delete;
-
- private:
-  void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
-  void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);
-
-  ::ros::NodeHandle node_handle_;
-  const double resolution_;
-
-  absl::Mutex mutex_;
-  ::ros::ServiceClient client_ GUARDED_BY(mutex_);
-  ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
-  ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
-  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
-  ::ros::WallTimer occupancy_grid_publisher_timer_;
-  std::string last_frame_id_;
-  ros::Time last_timestamp_;
-};
-
-Node::Node(const double resolution, const double publish_period_sec)
-    : resolution_(resolution),
-      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
-          kSubmapQueryServiceName)),
-      submap_list_subscriber_(node_handle_.subscribe(
-          kSubmapListTopic, kLatestOnlyPublisherQueueSize,
-          boost::function<void(
-              const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
-              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
-                HandleSubmapList(msg);
-              }))),
-      occupancy_grid_publisher_(
-          node_handle_.advertise<::nav_msgs::OccupancyGrid>(
-              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
-              true /* latched */)),
-      occupancy_grid_publisher_timer_(
-          node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
-                                       &Node::DrawAndPublish, this)) {}
-
-void Node::HandleSubmapList(
-    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
-  absl::MutexLock locker(&mutex_);
-
-  // We do not do any work if nobody listens.
-  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
-    return;
-  }
-
-  // Keep track of submap IDs that don't appear in the message anymore.
-  std::set<SubmapId> submap_ids_to_delete;
-  for (const auto& pair : submap_slices_) {
-    submap_ids_to_delete.insert(pair.first);
-  }
-
-  for (const auto& submap_msg : msg->submap) {
-    const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
-    submap_ids_to_delete.erase(id);
-    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
-        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
-      continue;
-    }
-    SubmapSlice& submap_slice = submap_slices_[id];
-    submap_slice.pose = ToRigid3d(submap_msg.pose);
-    submap_slice.metadata_version = submap_msg.submap_version;
-    if (submap_slice.surface != nullptr &&
-        submap_slice.version == submap_msg.submap_version) {
-      continue;
-    }
-
-    auto fetched_textures =
-        ::cartographer_ros::FetchSubmapTextures(id, &client_);
-    if (fetched_textures == nullptr) {
-      continue;
-    }
-    CHECK(!fetched_textures->textures.empty());
-    submap_slice.version = fetched_textures->version;
-
-    // We use the first texture only. By convention this is the highest
-    // resolution texture and that is the one we want to use to construct the
-    // map for ROS.
-    const auto fetched_texture = fetched_textures->textures.begin();
-    submap_slice.width = fetched_texture->width;
-    submap_slice.height = fetched_texture->height;
-    submap_slice.slice_pose = fetched_texture->slice_pose;
-    submap_slice.resolution = fetched_texture->resolution;
-    submap_slice.cairo_data.clear();
-    submap_slice.surface = ::cartographer::io::DrawTexture(
-        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
-        fetched_texture->width, fetched_texture->height,
-        &submap_slice.cairo_data);
-  }
-
-  // Delete all submaps that didn't appear in the message.
-  for (const auto& id : submap_ids_to_delete) {
-    submap_slices_.erase(id);
-  }
-
-  last_timestamp_ = msg->header.stamp;
-  last_frame_id_ = msg->header.frame_id;
-}
-
-void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
-  absl::MutexLock locker(&mutex_);
-  if (submap_slices_.empty() || last_frame_id_.empty()) {
-    return;
-  }
-  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
-  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
-      painted_slices, resolution_, last_frame_id_, last_timestamp_);
-  occupancy_grid_publisher_.publish(*msg_ptr);
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
-      << "Ignoring both frozen and unfrozen submaps makes no sense.";
-
-  ::ros::init(argc, argv, "cartographer_occupancy_grid_node");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-  ::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);
-
-  ::ros::spin();
-  ::ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/offline_node.cc

@@ -1,386 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer_ros/offline_node.h"
-
-#include <errno.h>
-#include <string.h>
-#ifndef WIN32
-#include <sys/resource.h>
-#endif
-#include <time.h>
-#include <chrono>
-
-#include "absl/strings/str_split.h"
-#include "cartographer_ros/node.h"
-#include "cartographer_ros/playable_bag.h"
-#include "cartographer_ros/urdf_reader.h"
-#include "gflags/gflags.h"
-#include "ros/callback_queue.h"
-#include "rosgraph_msgs/Clock.h"
-#include "tf2_ros/static_transform_broadcaster.h"
-#include "urdf/model.h"
-
-DEFINE_bool(collect_metrics, false,
-            "Activates the collection of runtime metrics. If activated, the "
-            "metrics can be accessed via a ROS service.");
-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_basenames, "",
-    "Comma-separated list of basenames, i.e. not containing any "
-    "directory prefix, of the configuration files for each trajectory. "
-    "The first configuration file will be used for node options. "
-    "If less configuration files are specified than trajectories, the "
-    "first file will be used the remaining trajectories.");
-DEFINE_string(
-    bag_filenames, "",
-    "Comma-separated list of bags to process. One bag per trajectory. "
-    "Any combination of simultaneous and sequential bags is supported.");
-DEFINE_string(urdf_filenames, "",
-              "Comma-separated list of one or more URDF files that contain "
-              "static links for the sensor configuration(s).");
-DEFINE_bool(use_bag_transforms, true,
-            "Whether to read, use and republish transforms from bags.");
-DEFINE_string(load_state_filename, "",
-              "If non-empty, filename of a .pbstream file to load, containing "
-              "a saved SLAM state.");
-DEFINE_bool(load_frozen_state, true,
-            "Load the saved state as frozen (non-optimized) trajectories.");
-DEFINE_bool(keep_running, false,
-            "Keep running the offline node after all messages from the bag "
-            "have been processed.");
-DEFINE_double(skip_seconds, 0,
-              "Optional amount of seconds to skip from the beginning "
-              "(i.e. when the earliest bag starts.). ");
-
-namespace cartographer_ros {
-
-constexpr char kClockTopic[] = "clock";
-constexpr char kTfStaticTopic[] = "/tf_static";
-constexpr char kTfTopic[] = "tf";
-constexpr double kClockPublishFrequencySec = 1. / 30.;
-constexpr int kSingleThreaded = 1;
-// We publish tf messages one second earlier than other messages. Under
-// the assumption of higher frequency tf this should ensure that tf can
-// always interpolate.
-const ::ros::Duration kDelay = ::ros::Duration(1.0);
-
-void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
-  CHECK(!FLAGS_configuration_directory.empty())
-      << "-configuration_directory is missing.";
-  CHECK(!FLAGS_configuration_basenames.empty())
-      << "-configuration_basenames is missing.";
-  CHECK(!(FLAGS_bag_filenames.empty() && FLAGS_load_state_filename.empty()))
-      << "-bag_filenames and -load_state_filename cannot both be unspecified.";
-  const std::vector<std::string> bag_filenames =
-      absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty());
-  cartographer_ros::NodeOptions node_options;
-  const std::vector<std::string> configuration_basenames =
-      absl::StrSplit(FLAGS_configuration_basenames, ',', absl::SkipEmpty());
-  std::vector<TrajectoryOptions> bag_trajectory_options(1);
-  std::tie(node_options, bag_trajectory_options.at(0)) =
-      LoadOptions(FLAGS_configuration_directory, configuration_basenames.at(0));
-
-  for (size_t bag_index = 1; bag_index < bag_filenames.size(); ++bag_index) {
-    TrajectoryOptions current_trajectory_options;
-    if (bag_index < configuration_basenames.size()) {
-      std::tie(std::ignore, current_trajectory_options) = LoadOptions(
-          FLAGS_configuration_directory, configuration_basenames.at(bag_index));
-    } else {
-      current_trajectory_options = bag_trajectory_options.at(0);
-    }
-    bag_trajectory_options.push_back(current_trajectory_options);
-  }
-  if (bag_filenames.size() > 0) {
-    CHECK_EQ(bag_trajectory_options.size(), bag_filenames.size());
-  }
-
-  // Since we preload the transform buffer, we should never have to wait for a
-  // transform. When we finish processing the bag, we will simply drop any
-  // remaining sensor data that cannot be transformed due to missing transforms.
-  node_options.lookup_transform_timeout_sec = 0.;
-
-  auto map_builder = map_builder_factory(node_options.map_builder_options);
-
-  const std::chrono::time_point<std::chrono::steady_clock> start_time =
-      std::chrono::steady_clock::now();
-
-  tf2_ros::Buffer tf_buffer;
-
-  std::vector<geometry_msgs::TransformStamped> urdf_transforms;
-  const std::vector<std::string> urdf_filenames =
-      absl::StrSplit(FLAGS_urdf_filenames, ',', absl::SkipEmpty());
-  for (const auto& urdf_filename : urdf_filenames) {
-    const auto current_urdf_transforms =
-        ReadStaticTransformsFromUrdf(urdf_filename, &tf_buffer);
-    urdf_transforms.insert(urdf_transforms.end(),
-                           current_urdf_transforms.begin(),
-                           current_urdf_transforms.end());
-  }
-
-  tf_buffer.setUsingDedicatedThread(true);
-
-  Node node(node_options, std::move(map_builder), &tf_buffer,
-            FLAGS_collect_metrics);
-  if (!FLAGS_load_state_filename.empty()) {
-    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
-  }
-
-  ::ros::Publisher tf_publisher =
-      node.node_handle()->advertise<tf2_msgs::TFMessage>(
-          kTfTopic, kLatestOnlyPublisherQueueSize);
-
-  ::tf2_ros::StaticTransformBroadcaster static_tf_broadcaster;
-
-  ::ros::Publisher clock_publisher =
-      node.node_handle()->advertise<rosgraph_msgs::Clock>(
-          kClockTopic, kLatestOnlyPublisherQueueSize);
-
-  if (urdf_transforms.size() > 0) {
-    static_tf_broadcaster.sendTransform(urdf_transforms);
-  }
-
-  ros::AsyncSpinner async_spinner(kSingleThreaded);
-  async_spinner.start();
-  rosgraph_msgs::Clock clock;
-  auto clock_republish_timer = node.node_handle()->createWallTimer(
-      ::ros::WallDuration(kClockPublishFrequencySec),
-      [&clock_publisher, &clock](const ::ros::WallTimerEvent&) {
-        clock_publisher.publish(clock);
-      },
-      false /* oneshot */, false /* autostart */);
-
-  std::vector<
-      std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
-      bag_expected_sensor_ids;
-  if (configuration_basenames.size() == 1) {
-    const auto current_bag_expected_sensor_ids =
-        node.ComputeDefaultSensorIdsForMultipleBags(
-            {bag_trajectory_options.front()});
-    bag_expected_sensor_ids = {bag_filenames.size(),
-                               current_bag_expected_sensor_ids.front()};
-  } else {
-    bag_expected_sensor_ids =
-        node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
-  }
-  CHECK_EQ(bag_expected_sensor_ids.size(), bag_filenames.size());
-
-  std::map<std::pair<int /* bag_index */, std::string>,
-           cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-      bag_topic_to_sensor_id;
-  PlayableBagMultiplexer playable_bag_multiplexer;
-  for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
-       ++current_bag_index) {
-    const std::string& bag_filename = bag_filenames.at(current_bag_index);
-    if (!::ros::ok()) {
-      return;
-    }
-    for (const auto& expected_sensor_id :
-         bag_expected_sensor_ids.at(current_bag_index)) {
-      const auto bag_resolved_topic = std::make_pair(
-          static_cast<int>(current_bag_index),
-          node.node_handle()->resolveName(expected_sensor_id.id));
-      if (bag_topic_to_sensor_id.count(bag_resolved_topic) != 0) {
-        LOG(ERROR) << "Sensor " << expected_sensor_id.id << " of bag "
-                   << current_bag_index << " resolves to topic "
-                   << bag_resolved_topic.second << " which is already used by "
-                   << " sensor "
-                   << bag_topic_to_sensor_id.at(bag_resolved_topic).id;
-      }
-      bag_topic_to_sensor_id[bag_resolved_topic] = expected_sensor_id;
-    }
-
-    playable_bag_multiplexer.AddPlayableBag(PlayableBag(
-        bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
-        // PlayableBag::FilteringEarlyMessageHandler is used to get an early
-        // peek at the tf messages in the bag and insert them into 'tf_buffer'.
-        // When a message is retrieved by GetNextMessage() further below,
-        // we will have already inserted further 'kDelay' seconds worth of
-        // transforms into 'tf_buffer' via this lambda.
-        [&tf_publisher, &tf_buffer](const rosbag::MessageInstance& msg) {
-          if (msg.isType<tf2_msgs::TFMessage>()) {
-            if (FLAGS_use_bag_transforms) {
-              const auto tf_message = msg.instantiate<tf2_msgs::TFMessage>();
-              tf_publisher.publish(tf_message);
-
-              for (const auto& transform : tf_message->transforms) {
-                try {
-                  // We need to keep 'tf_buffer' small because it becomes very
-                  // inefficient otherwise. We make sure that tf_messages are
-                  // published before any data messages, so that tf lookups
-                  // always work.
-                  tf_buffer.setTransform(transform, "unused_authority",
-                                         msg.getTopic() == kTfStaticTopic);
-                } catch (const tf2::TransformException& ex) {
-                  LOG(WARNING) << ex.what();
-                }
-              }
-            }
-            // Tell 'PlayableBag' to filter the tf message since there is no
-            // further use for it.
-            return false;
-          } else {
-            return true;
-          }
-        }));
-  }
-
-  std::set<std::string> bag_topics;
-  std::stringstream bag_topics_string;
-  for (const auto& topic : playable_bag_multiplexer.topics()) {
-    std::string resolved_topic = node.node_handle()->resolveName(topic, false);
-    bag_topics.insert(resolved_topic);
-    bag_topics_string << resolved_topic << ",";
-  }
-  bool print_topics = false;
-  for (const auto& entry : bag_topic_to_sensor_id) {
-    const std::string& resolved_topic = entry.first.second;
-    if (bag_topics.count(resolved_topic) == 0) {
-      LOG(WARNING) << "Expected resolved topic \"" << resolved_topic
-                   << "\" not found in bag file(s).";
-      print_topics = true;
-    }
-  }
-  if (print_topics) {
-    LOG(WARNING) << "Available topics in bag file(s) are "
-                 << bag_topics_string.str();
-  }
-
-  std::unordered_map<int, int> bag_index_to_trajectory_id;
-  const ros::Time begin_time =
-      // If no bags were loaded, we cannot peek the time of first message.
-      playable_bag_multiplexer.IsMessageAvailable()
-          ? playable_bag_multiplexer.PeekMessageTime()
-          : ros::Time();
-  while (playable_bag_multiplexer.IsMessageAvailable()) {
-    if (!::ros::ok()) {
-      return;
-    }
-
-    const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
-    const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
-    const int bag_index = std::get<1>(next_msg_tuple);
-    const bool is_last_message_in_bag = std::get<2>(next_msg_tuple);
-
-    if (msg.getTime() < (begin_time + ros::Duration(FLAGS_skip_seconds))) {
-      continue;
-    }
-
-    int trajectory_id;
-    // Lazily add trajectories only when the first message arrives in order
-    // to avoid blocking the sensor queue.
-    if (bag_index_to_trajectory_id.count(bag_index) == 0) {
-      trajectory_id =
-          node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index),
-                                    bag_trajectory_options.at(bag_index));
-      CHECK(bag_index_to_trajectory_id
-                .emplace(std::piecewise_construct,
-                         std::forward_as_tuple(bag_index),
-                         std::forward_as_tuple(trajectory_id))
-                .second);
-      LOG(INFO) << "Assigned trajectory " << trajectory_id << " to bag "
-                << bag_filenames.at(bag_index);
-    } else {
-      trajectory_id = bag_index_to_trajectory_id.at(bag_index);
-    }
-
-    const auto bag_topic = std::make_pair(
-        bag_index,
-        node.node_handle()->resolveName(msg.getTopic(), false /* resolve */));
-    auto it = bag_topic_to_sensor_id.find(bag_topic);
-    if (it != bag_topic_to_sensor_id.end()) {
-      const std::string& sensor_id = it->second.id;
-      if (msg.isType<sensor_msgs::LaserScan>()) {
-        node.HandleLaserScanMessage(trajectory_id, sensor_id,
-                                    msg.instantiate<sensor_msgs::LaserScan>());
-      }
-      if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
-        node.HandleMultiEchoLaserScanMessage(
-            trajectory_id, sensor_id,
-            msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
-      }
-      if (msg.isType<sensor_msgs::PointCloud2>()) {
-        node.HandlePointCloud2Message(
-            trajectory_id, sensor_id,
-            msg.instantiate<sensor_msgs::PointCloud2>());
-      }
-      if (msg.isType<sensor_msgs::Imu>()) {
-        node.HandleImuMessage(trajectory_id, sensor_id,
-                              msg.instantiate<sensor_msgs::Imu>());
-      }
-      if (msg.isType<nav_msgs::Odometry>()) {
-        node.HandleOdometryMessage(trajectory_id, sensor_id,
-                                   msg.instantiate<nav_msgs::Odometry>());
-      }
-      if (msg.isType<sensor_msgs::NavSatFix>()) {
-        node.HandleNavSatFixMessage(trajectory_id, sensor_id,
-                                    msg.instantiate<sensor_msgs::NavSatFix>());
-      }
-      if (msg.isType<cartographer_ros_msgs::LandmarkList>()) {
-        node.HandleLandmarkMessage(
-            trajectory_id, sensor_id,
-            msg.instantiate<cartographer_ros_msgs::LandmarkList>());
-      }
-    }
-    clock.clock = msg.getTime();
-    clock_publisher.publish(clock);
-
-    if (is_last_message_in_bag) {
-      node.FinishTrajectory(trajectory_id);
-    }
-  }
-
-  // Ensure the clock is republished after the bag has been finished, during the
-  // final optimization, serialization, and optional indefinite spinning at the
-  // end.
-  clock_republish_timer.start();
-  node.RunFinalOptimization();
-
-  const std::chrono::time_point<std::chrono::steady_clock> end_time =
-      std::chrono::steady_clock::now();
-  const double wall_clock_seconds =
-      std::chrono::duration_cast<std::chrono::duration<double>>(end_time -
-                                                                start_time)
-          .count();
-
-  LOG(INFO) << "Elapsed wall clock time: " << wall_clock_seconds << " s";
-#ifdef __linux__
-  timespec cpu_timespec = {};
-  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &cpu_timespec);
-  LOG(INFO) << "Elapsed CPU time: "
-            << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
-  rusage usage;
-  CHECK_EQ(getrusage(RUSAGE_SELF, &usage), 0) << strerror(errno);
-  LOG(INFO) << "Peak memory usage: " << usage.ru_maxrss << " KiB";
-#endif
-
-  if (::ros::ok() && bag_filenames.size() > 0) {
-    const std::string output_filename = bag_filenames.front();
-    const std::string suffix = ".pbstream";
-    const std::string state_output_filename = output_filename + suffix;
-    LOG(INFO) << "Writing state to '" << state_output_filename << "'...";
-    node.SerializeState(state_output_filename,
-                        true /* include_unfinished_submaps */);
-  }
-  if (FLAGS_keep_running) {
-    ::ros::waitForShutdown();
-  }
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/offline_node.h

@@ -1,38 +0,0 @@
-/*
- * Copyright 2018 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_CARTOGRAPHER_ROS_OFFLINE_NODE_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H
-
-#include <functional>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "cartographer/mapping/map_builder_interface.h"
-#include "cartographer_ros/node_options.h"
-
-namespace cartographer_ros {
-
-using MapBuilderFactory =
-    std::function<std::unique_ptr<::cartographer::mapping::MapBuilderInterface>(
-        const ::cartographer::mapping::proto::MapBuilderOptions&)>;
-
-void RunOfflineNode(const MapBuilderFactory& map_builder_factory);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_OFFLINE_NODE_H

cartographer_ros/cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -1,42 +0,0 @@
-/*
- * Copyright 2017 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 "cartographer/mapping/map_builder.h"
-#include "cartographer_ros/offline_node.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "gflags/gflags.h"
-#include "ros/ros.h"
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  ::ros::init(argc, argv, "cartographer_offline_node");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-
-  const cartographer_ros::MapBuilderFactory map_builder_factory =
-      [](const ::cartographer::mapping::proto::MapBuilderOptions&
-             map_builder_options) {
-        return absl::make_unique< ::cartographer::mapping::MapBuilder>(
-            map_builder_options);
-      };
-
-  cartographer_ros::RunOfflineNode(map_builder_factory);
-
-  ::ros::shutdown();
-}

cartographer_ros/cartographer_ros/cartographer_ros/pbstream_map_publisher_main.cc

@@ -1,99 +0,0 @@
-/*
- * Copyright 2018 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.
- */
-
-// Publishes a frozen nav_msgs/OccupancyGrid map from serialized submaps.
-
-#include <map>
-#include <string>
-
-#include "cartographer/io/proto_stream.h"
-#include "cartographer/io/proto_stream_deserializer.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/submap_2d.h"
-#include "cartographer/mapping/3d/submap_3d.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/node_constants.h"
-#include "cartographer_ros/ros_log_sink.h"
-#include "cartographer_ros/ros_map.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "ros/ros.h"
-
-DEFINE_string(pbstream_filename, "",
-              "Filename of a pbstream to draw a map from.");
-DEFINE_string(map_topic, "map", "Name of the published map topic.");
-DEFINE_string(map_frame_id, "map", "Frame ID of the published map.");
-DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
-
-namespace cartographer_ros {
-namespace {
-
-std::unique_ptr<nav_msgs::OccupancyGrid> LoadOccupancyGridMsg(
-    const std::string& pbstream_filename, const double resolution) {
-  ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
-  ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
-
-  LOG(INFO) << "Loading submap slices from serialized data.";
-  std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
-      submap_slices;
-  ::cartographer::mapping::ValueConversionTables conversion_tables;
-  ::cartographer::io::DeserializeAndFillSubmapSlices(
-      &deserializer, &submap_slices, &conversion_tables);
-  CHECK(reader.eof());
-
-  LOG(INFO) << "Generating combined map image from submap slices.";
-  const auto painted_slices =
-      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
-  return CreateOccupancyGridMsg(painted_slices, resolution, FLAGS_map_frame_id,
-                                ros::Time::now());
-}
-
-void Run(const std::string& pbstream_filename, const std::string& map_topic,
-         const std::string& map_frame_id, const double resolution) {
-  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr =
-      LoadOccupancyGridMsg(pbstream_filename, resolution);
-
-  ::ros::NodeHandle node_handle("");
-  ::ros::Publisher pub = node_handle.advertise<nav_msgs::OccupancyGrid>(
-      map_topic, kLatestOnlyPublisherQueueSize, true /*latched */);
-
-  LOG(INFO) << "Publishing occupancy grid topic " << map_topic
-            << " (frame_id: " << map_frame_id
-            << ", resolution:" << std::to_string(resolution) << ").";
-  pub.publish(*msg_ptr);
-  ::ros::spin();
-  ::ros::shutdown();
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
-
-  ::ros::init(argc, argv, "cartographer_pbstream_map_publisher");
-  ::ros::start();
-
-  cartographer_ros::ScopedRosLogSink ros_log_sink;
-
-  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_topic,
-                          FLAGS_map_frame_id, FLAGS_resolution);
-}

cartographer_ros/cartographer_ros/cartographer_ros/pbstream_to_ros_map_main.cc

@@ -1,81 +0,0 @@
-/*
- * Copyright 2017 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 <map>
-#include <string>
-
-#include "cartographer/io/proto_stream.h"
-#include "cartographer/io/proto_stream_deserializer.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/submap_2d.h"
-#include "cartographer/mapping/3d/submap_3d.h"
-#include "cartographer_ros/ros_map.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-
-DEFINE_string(pbstream_filename, "",
-              "Filename of a pbstream to draw a map from.");
-DEFINE_string(map_filestem, "map", "Stem of the output files.");
-DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
-
-namespace cartographer_ros {
-namespace {
-
-void Run(const std::string& pbstream_filename, const std::string& map_filestem,
-         const double resolution) {
-  ::cartographer::io::ProtoStreamReader reader(pbstream_filename);
-  ::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
-
-  LOG(INFO) << "Loading submap slices from serialized data.";
-  std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
-      submap_slices;
-  ::cartographer::mapping::ValueConversionTables conversion_tables;
-  ::cartographer::io::DeserializeAndFillSubmapSlices(
-      &deserializer, &submap_slices, &conversion_tables);
-  CHECK(reader.eof());
-
-  LOG(INFO) << "Generating combined map image from submap slices.";
-  auto result =
-      ::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
-
-  ::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
-
-  ::cartographer::io::Image image(std::move(result.surface));
-  WritePgm(image, resolution, &pgm_writer);
-
-  const Eigen::Vector2d origin(
-      -result.origin.x() * resolution,
-      (result.origin.y() - image.height()) * resolution);
-
-  ::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
-  WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  FLAGS_alsologtostderr = true;
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
-  CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing.";
-
-  ::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem,
-                          FLAGS_resolution);
-}

cartographer_ros/cartographer_ros/cartographer_ros/playable_bag.cc

@@ -1,170 +0,0 @@
-/*
- * Copyright 2018 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 "cartographer_ros/playable_bag.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer_ros/node_constants.h"
-#include "glog/logging.h"
-#include "tf2_msgs/TFMessage.h"
-
-namespace cartographer_ros {
-
-PlayableBag::PlayableBag(
-    const std::string& bag_filename, const int bag_id,
-    const ros::Time start_time, const ros::Time end_time,
-    const ros::Duration buffer_delay,
-    FilteringEarlyMessageHandler filtering_early_message_handler)
-    : bag_(absl::make_unique<rosbag::Bag>(bag_filename, rosbag::bagmode::Read)),
-      view_(absl::make_unique<rosbag::View>(*bag_, start_time, end_time)),
-      view_iterator_(view_->begin()),
-      finished_(false),
-      bag_id_(bag_id),
-      bag_filename_(bag_filename),
-      duration_in_seconds_(
-          (view_->getEndTime() - view_->getBeginTime()).toSec()),
-      message_counter_(0),
-      buffer_delay_(buffer_delay),
-      filtering_early_message_handler_(
-          std::move(filtering_early_message_handler)) {
-  AdvanceUntilMessageAvailable();
-  for (const auto* connection_info : view_->getConnections()) {
-    topics_.insert(connection_info->topic);
-  }
-}
-
-ros::Time PlayableBag::PeekMessageTime() const {
-  CHECK(IsMessageAvailable());
-  return buffered_messages_.front().getTime();
-}
-
-std::tuple<ros::Time, ros::Time> PlayableBag::GetBeginEndTime() const {
-  return std::make_tuple(view_->getBeginTime(), view_->getEndTime());
-}
-
-rosbag::MessageInstance PlayableBag::GetNextMessage(
-    cartographer_ros_msgs::BagfileProgress* progress) {
-  CHECK(IsMessageAvailable());
-  const rosbag::MessageInstance msg = buffered_messages_.front();
-  buffered_messages_.pop_front();
-  AdvanceUntilMessageAvailable();
-  double processed_seconds = (msg.getTime() - view_->getBeginTime()).toSec();
-  if ((message_counter_ % 10000) == 0) {
-    LOG(INFO) << "Processed " << processed_seconds << " of "
-              << duration_in_seconds_ << " seconds of bag " << bag_filename_;
-  }
-
-  if (progress) {
-    progress->current_bagfile_name = bag_filename_;
-    progress->current_bagfile_id = bag_id_;
-    progress->total_messages = view_->size();
-    progress->processed_messages = message_counter_;
-    progress->total_seconds = duration_in_seconds_;
-    progress->processed_seconds = processed_seconds;
-  }
-
-  return msg;
-}
-
-bool PlayableBag::IsMessageAvailable() const {
-  return !buffered_messages_.empty() &&
-         (buffered_messages_.front().getTime() <
-          buffered_messages_.back().getTime() - buffer_delay_);
-}
-
-int PlayableBag::bag_id() const { return bag_id_; }
-
-void PlayableBag::AdvanceOneMessage() {
-  CHECK(!finished_);
-  if (view_iterator_ == view_->end()) {
-    finished_ = true;
-    return;
-  }
-  rosbag::MessageInstance& msg = *view_iterator_;
-  if (!filtering_early_message_handler_ ||
-      filtering_early_message_handler_(msg)) {
-    buffered_messages_.push_back(msg);
-  }
-  ++view_iterator_;
-  ++message_counter_;
-}
-
-void PlayableBag::AdvanceUntilMessageAvailable() {
-  if (finished_) {
-    return;
-  }
-  do {
-    AdvanceOneMessage();
-  } while (!finished_ && !IsMessageAvailable());
-}
-
-PlayableBagMultiplexer::PlayableBagMultiplexer() : pnh_("~") {
-  bag_progress_pub_ = pnh_.advertise<cartographer_ros_msgs::BagfileProgress>(
-      "bagfile_progress", 10);
-  progress_pub_interval_ = pnh_.param("bagfile_progress_pub_interval", 10.0);
-}
-
-void PlayableBagMultiplexer::AddPlayableBag(PlayableBag playable_bag) {
-  for (const auto& topic : playable_bag.topics()) {
-    topics_.insert(topic);
-  }
-  playable_bags_.push_back(std::move(playable_bag));
-  CHECK(playable_bags_.back().IsMessageAvailable());
-  next_message_queue_.emplace(
-      BagMessageItem{playable_bags_.back().PeekMessageTime(),
-                     static_cast<int>(playable_bags_.size() - 1)});
-  bag_progress_time_map_[playable_bag.bag_id()] = ros::Time::now();
-}
-
-bool PlayableBagMultiplexer::IsMessageAvailable() const {
-  return !next_message_queue_.empty();
-}
-
-std::tuple<rosbag::MessageInstance, int, bool>
-PlayableBagMultiplexer::GetNextMessage() {
-  CHECK(IsMessageAvailable());
-  const int current_bag_index = next_message_queue_.top().bag_index;
-  PlayableBag& current_bag = playable_bags_.at(current_bag_index);
-  cartographer_ros_msgs::BagfileProgress progress;
-  rosbag::MessageInstance msg = current_bag.GetNextMessage(&progress);
-  const bool publish_progress =
-      current_bag.finished() ||
-      ros::Time::now() - bag_progress_time_map_[current_bag.bag_id()] >=
-          ros::Duration(progress_pub_interval_);
-  if (bag_progress_pub_.getNumSubscribers() > 0 && publish_progress) {
-    progress.total_bagfiles = playable_bags_.size();
-    if (current_bag.finished()) {
-      progress.processed_seconds = current_bag.duration_in_seconds();
-    }
-    bag_progress_pub_.publish(progress);
-    bag_progress_time_map_[current_bag.bag_id()] = ros::Time::now();
-  }
-  CHECK_EQ(msg.getTime(), next_message_queue_.top().message_timestamp);
-  next_message_queue_.pop();
-  if (current_bag.IsMessageAvailable()) {
-    next_message_queue_.emplace(
-        BagMessageItem{current_bag.PeekMessageTime(), current_bag_index});
-  }
-  return std::make_tuple(std::move(msg), current_bag.bag_id(),
-                         !current_bag.IsMessageAvailable());
-}
-
-ros::Time PlayableBagMultiplexer::PeekMessageTime() const {
-  CHECK(IsMessageAvailable());
-  return next_message_queue_.top().message_timestamp;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/playable_bag.h

@@ -1,116 +0,0 @@
-/*
- * Copyright 2018 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_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H
-#include <functional>
-#include <queue>
-
-#include <cartographer_ros_msgs/BagfileProgress.h>
-#include <ros/node_handle.h>
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "tf2_ros/buffer.h"
-
-namespace cartographer_ros {
-
-class PlayableBag {
- public:
-  // Handles messages early, i.e. when they are about to enter the buffer.
-  // Returns a boolean indicating whether the message should enter the buffer.
-  using FilteringEarlyMessageHandler =
-      std::function<bool /* forward_message_to_buffer */ (
-          const rosbag::MessageInstance&)>;
-
-  PlayableBag(const std::string& bag_filename, int bag_id, ros::Time start_time,
-              ros::Time end_time, ros::Duration buffer_delay,
-              FilteringEarlyMessageHandler filtering_early_message_handler);
-
-  ros::Time PeekMessageTime() const;
-  rosbag::MessageInstance GetNextMessage(
-      cartographer_ros_msgs::BagfileProgress* progress);
-  bool IsMessageAvailable() const;
-  std::tuple<ros::Time, ros::Time> GetBeginEndTime() const;
-
-  int bag_id() const;
-  std::set<std::string> topics() const { return topics_; }
-  double duration_in_seconds() const { return duration_in_seconds_; }
-  bool finished() const { return finished_; }
-
- private:
-  void AdvanceOneMessage();
-  void AdvanceUntilMessageAvailable();
-
-  std::unique_ptr<rosbag::Bag> bag_;
-  std::unique_ptr<rosbag::View> view_;
-  rosbag::View::const_iterator view_iterator_;
-  bool finished_;
-  const int bag_id_;
-  const std::string bag_filename_;
-  const double duration_in_seconds_;
-  int message_counter_;
-  std::deque<rosbag::MessageInstance> buffered_messages_;
-  const ::ros::Duration buffer_delay_;
-  FilteringEarlyMessageHandler filtering_early_message_handler_;
-  std::set<std::string> topics_;
-};
-
-class PlayableBagMultiplexer {
- public:
-  PlayableBagMultiplexer();
-  void AddPlayableBag(PlayableBag playable_bag);
-
-  // Returns the next message from the multiplexed (merge-sorted) message
-  // stream, along with the bag id corresponding to the message, and whether
-  // this was the last message in that bag.
-  std::tuple<rosbag::MessageInstance, int /* bag_id */,
-             bool /* is_last_message_in_bag */>
-  GetNextMessage();
-
-  bool IsMessageAvailable() const;
-  ros::Time PeekMessageTime() const;
-
-  std::set<std::string> topics() const { return topics_; }
-
- private:
-  struct BagMessageItem {
-    ros::Time message_timestamp;
-    int bag_index;
-    struct TimestampIsGreater {
-      bool operator()(const BagMessageItem& l, const BagMessageItem& r) {
-        return l.message_timestamp > r.message_timestamp;
-      }
-    };
-  };
-
-  ros::NodeHandle pnh_;
-  // Publishes information about the bag-file(s) processing and its progress
-  ros::Publisher bag_progress_pub_;
-  // Map between bagfile id and the last time when its progress was published
-  std::map<int, ros::Time> bag_progress_time_map_;
-  // The time interval of publishing bag-file(s) processing in seconds
-  double progress_pub_interval_;
-
-  std::vector<PlayableBag> playable_bags_;
-  std::priority_queue<BagMessageItem, std::vector<BagMessageItem>,
-                      BagMessageItem::TimestampIsGreater>
-      next_message_queue_;
-  std::set<std::string> topics_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_PLAYABLE_BAG_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.cc

@@ -1,76 +0,0 @@
-/*
- * 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 "cartographer_ros/ros_log_sink.h"
-
-#include <chrono>
-#include <cstring>
-#include <string>
-#include <thread>
-
-#include "glog/log_severity.h"
-#include "ros/console.h"
-
-namespace cartographer_ros {
-
-namespace {
-
-const char* GetBasename(const char* filepath) {
-  const char* base = std::strrchr(filepath, '/');
-  return base ? (base + 1) : filepath;
-}
-
-}  // namespace
-
-ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); }
-ScopedRosLogSink::~ScopedRosLogSink() { RemoveLogSink(this); }
-
-void ScopedRosLogSink::send(const ::google::LogSeverity severity,
-                            const char* const filename,
-                            const char* const base_filename, const int line,
-                            const struct std::tm* const tm_time,
-                            const char* const message,
-                            const size_t message_len) {
-  const std::string message_string = ::google::LogSink::ToString(
-      severity, GetBasename(filename), line, tm_time, message, message_len);
-  switch (severity) {
-    case ::google::GLOG_INFO:
-      ROS_INFO_STREAM(message_string);
-      break;
-
-    case ::google::GLOG_WARNING:
-      ROS_WARN_STREAM(message_string);
-      break;
-
-    case ::google::GLOG_ERROR:
-      ROS_ERROR_STREAM(message_string);
-      break;
-
-    case ::google::GLOG_FATAL:
-      ROS_FATAL_STREAM(message_string);
-      will_die_ = true;
-      break;
-  }
-}
-
-void ScopedRosLogSink::WaitTillSent() {
-  if (will_die_) {
-    // Give ROS some time to actually publish our message.
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-  }
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.h

@@ -1,45 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H
-
-#include <ctime>
-
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-// Makes Google logging use ROS logging for output while an instance of this
-// class exists.
-class ScopedRosLogSink : public ::google::LogSink {
- public:
-  ScopedRosLogSink();
-  ~ScopedRosLogSink() override;
-
-  void send(::google::LogSeverity severity, const char* filename,
-            const char* base_filename, int line, const struct std::tm* tm_time,
-            const char* message, size_t message_len) override;
-
-  void WaitTillSent() override;
-
- private:
-  bool will_die_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_LOG_SINK_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_map.cc

@@ -1,49 +0,0 @@
-/*
- * Copyright 2017 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 "cartographer_ros/ros_map.h"
-
-namespace cartographer_ros {
-
-void WritePgm(const ::cartographer::io::Image& image, const double resolution,
-              ::cartographer::io::FileWriter* file_writer) {
-  const std::string header = "P5\n# Cartographer map; " +
-                             std::to_string(resolution) + " m/pixel\n" +
-                             std::to_string(image.width()) + " " +
-                             std::to_string(image.height()) + "\n255\n";
-  file_writer->Write(header.data(), header.size());
-  for (int y = 0; y < image.height(); ++y) {
-    for (int x = 0; x < image.width(); ++x) {
-      const char color = image.GetPixel(x, y)[0];
-      file_writer->Write(&color, 1);
-    }
-  }
-}
-
-void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
-               const std::string& pgm_filename,
-               ::cartographer::io::FileWriter* file_writer) {
-  // Magic constants taken directly from ros map_saver code:
-  // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
-  const std::string output =
-      "image: " + pgm_filename + "\n" +
-      "resolution: " + std::to_string(resolution) + "\n" + "origin: [" +
-      std::to_string(origin.x()) + ", " + std::to_string(origin.y()) +
-      ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";
-  file_writer->Write(output.data(), output.size());
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_map.h

@@ -1,41 +0,0 @@
-/*
- * Copyright 2017 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_CARTOGRAPHER_ROS_ROS_MAP_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H
-
-#include <string>
-
-#include "Eigen/Core"
-#include "cartographer/io/file_writer.h"
-#include "cartographer/io/image.h"
-#include "cartographer/mapping/2d/map_limits.h"
-
-namespace cartographer_ros {
-
-// Write 'image' as a pgm into 'file_writer'. The resolution is used in the
-// comment only'
-void WritePgm(const ::cartographer::io::Image& image, const double resolution,
-              ::cartographer::io::FileWriter* file_writer);
-
-// Write the corresponding yaml into 'file_writer'.
-void WriteYaml(const double resolution, const Eigen::Vector2d& origin,
-               const std::string& pgm_filename,
-               ::cartographer::io::FileWriter* file_writer);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_H

cartographer_ros/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc

@@ -1,96 +0,0 @@
-/*
- * 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 "cartographer_ros/ros_map_writing_points_processor.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer/io/image.h"
-#include "cartographer/io/probability_grid_points_processor.h"
-#include "cartographer_ros/ros_map.h"
-
-namespace cartographer_ros {
-
-RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
-    const double resolution,
-    const ::cartographer::mapping::proto::
-        ProbabilityGridRangeDataInserterOptions2D& range_data_inserter_options,
-    ::cartographer::io::FileWriterFactory file_writer_factory,
-    const std::string& filestem,
-    ::cartographer::io::PointsProcessor* const next)
-    : filestem_(filestem),
-      next_(next),
-      file_writer_factory_(file_writer_factory),
-      range_data_inserter_(range_data_inserter_options),
-      probability_grid_(::cartographer::io::CreateProbabilityGrid(
-          resolution, &conversion_tables_)) {}
-
-std::unique_ptr<RosMapWritingPointsProcessor>
-RosMapWritingPointsProcessor::FromDictionary(
-    ::cartographer::io::FileWriterFactory file_writer_factory,
-    ::cartographer::common::LuaParameterDictionary* const dictionary,
-    ::cartographer::io::PointsProcessor* const next) {
-  return absl::make_unique<RosMapWritingPointsProcessor>(
-      dictionary->GetDouble("resolution"),
-      ::cartographer::mapping::CreateProbabilityGridRangeDataInserterOptions2D(
-          dictionary->GetDictionary("range_data_inserter").get()),
-      file_writer_factory, dictionary->GetString("filestem"), next);
-}
-
-void RosMapWritingPointsProcessor::Process(
-    std::unique_ptr<::cartographer::io::PointsBatch> batch) {
-  range_data_inserter_.Insert({batch->origin, batch->points, {}},
-                              &probability_grid_);
-  next_->Process(std::move(batch));
-}
-
-::cartographer::io::PointsProcessor::FlushResult
-RosMapWritingPointsProcessor::Flush() {
-  Eigen::Array2i offset;
-  std::unique_ptr<::cartographer::io::Image> image =
-      ::cartographer::io::DrawProbabilityGrid(probability_grid_, &offset);
-  if (image != nullptr) {
-    auto pgm_writer = file_writer_factory_(filestem_ + ".pgm");
-    const std::string pgm_filename = pgm_writer->GetFilename();
-    const auto& limits = probability_grid_.limits();
-    image->Rotate90DegreesClockwise();
-
-    WritePgm(*image, limits.resolution(), pgm_writer.get());
-    CHECK(pgm_writer->Close());
-
-    const Eigen::Vector2d origin(
-        limits.max().x() - (offset.y() + image->width()) * limits.resolution(),
-        limits.max().y() -
-            (offset.x() + image->height()) * limits.resolution());
-    auto yaml_writer = file_writer_factory_(filestem_ + ".yaml");
-    WriteYaml(limits.resolution(), origin, pgm_filename, yaml_writer.get());
-    CHECK(yaml_writer->Close());
-  }
-
-  switch (next_->Flush()) {
-    case FlushResult::kRestartStream:
-      LOG(FATAL) << "ROS map writing must be configured to occur after any "
-                    "stages that require multiple passes.";
-
-    case FlushResult::kFinished:
-      return FlushResult::kFinished;
-  }
-  LOG(FATAL);
-  // The following unreachable return statement is needed to avoid a GCC bug
-  // described at https://gcc.gnu.org/bugzilla/show_bug.cgi?id=81508
-  return FlushResult::kFinished;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h

@@ -1,69 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H
-
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/io/file_writer.h"
-#include "cartographer/io/points_processor.h"
-#include "cartographer/mapping/2d/probability_grid.h"
-#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
-#include "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.pb.h"
-#include "cartographer/mapping/value_conversion_tables.h"
-
-namespace cartographer_ros {
-
-// Very similar to Cartographer's ProbabilityGridPointsProcessor, but writes
-// out a PGM and YAML suitable for ROS map server to consume.
-class RosMapWritingPointsProcessor
-    : public ::cartographer::io::PointsProcessor {
- public:
-  constexpr static const char* kConfigurationFileActionName = "write_ros_map";
-  RosMapWritingPointsProcessor(
-      double resolution,
-      const ::cartographer::mapping::proto::
-          ProbabilityGridRangeDataInserterOptions2D&
-              range_data_inserter_options,
-      ::cartographer::io::FileWriterFactory file_writer_factory,
-      const std::string& filestem, PointsProcessor* next);
-  RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
-  RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
-      delete;
-
-  static std::unique_ptr<RosMapWritingPointsProcessor> FromDictionary(
-      ::cartographer::io::FileWriterFactory file_writer_factory,
-      ::cartographer::common::LuaParameterDictionary* dictionary,
-      PointsProcessor* next);
-
-  ~RosMapWritingPointsProcessor() override {}
-
-  void Process(std::unique_ptr<::cartographer::io::PointsBatch> batch) override;
-  FlushResult Flush() override;
-
- private:
-  const std::string filestem_;
-  PointsProcessor* const next_;
-  ::cartographer::io::FileWriterFactory file_writer_factory_;
-  ::cartographer::mapping::ProbabilityGridRangeDataInserter2D
-      range_data_inserter_;
-  ::cartographer::mapping::ValueConversionTables conversion_tables_;
-  ::cartographer::mapping::ProbabilityGrid probability_grid_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_ROS_MAP_WRITING_POINTS_PROCESSOR_H

cartographer_ros/cartographer_ros/cartographer_ros/rosbag_validate_main.cc

@@ -1,425 +0,0 @@
-/*
- * Copyright 2017 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 <fstream>
-#include <iostream>
-#include <map>
-#include <set>
-#include <string>
-
-#include "absl/memory/memory.h"
-#include "cartographer/common/histogram.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "gflags/gflags.h"
-#include "glog/logging.h"
-#include "nav_msgs/Odometry.h"
-#include "ros/ros.h"
-#include "ros/time.h"
-#include "rosbag/bag.h"
-#include "rosbag/view.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/PointCloud2.h"
-#include "tf2_eigen/tf2_eigen.h"
-#include "tf2_msgs/TFMessage.h"
-#include "tf2_ros/buffer.h"
-#include "urdf/model.h"
-
-DEFINE_string(bag_filename, "", "Bag to process.");
-DEFINE_bool(dump_timing, false,
-            "Dump per-sensor timing information in files called "
-            "timing_<frame_id>.csv in the current directory.");
-
-namespace cartographer_ros {
-namespace {
-
-struct FrameProperties {
-  ros::Time last_timestamp;
-  std::string topic;
-  std::vector<float> time_deltas;
-  std::unique_ptr<std::ofstream> timing_file;
-  std::string data_type;
-};
-
-const double kMinLinearAcceleration = 3.;
-const double kMaxLinearAcceleration = 30.;
-const double kTimeDeltaSerializationSensorWarning = 0.1;
-const double kTimeDeltaSerializationSensorError = 0.5;
-const double kMinAverageAcceleration = 9.5;
-const double kMaxAverageAcceleration = 10.5;
-const double kMaxGapPointsData = 0.1;
-const double kMaxGapImuData = 0.05;
-const std::set<std::string> kPointDataTypes = {
-    std::string(
-        ros::message_traits::DataType<sensor_msgs::PointCloud2>::value()),
-    std::string(ros::message_traits::DataType<
-                sensor_msgs::MultiEchoLaserScan>::value()),
-    std::string(
-        ros::message_traits::DataType<sensor_msgs::LaserScan>::value())};
-
-std::unique_ptr<std::ofstream> CreateTimingFile(const std::string& frame_id) {
-  auto timing_file = absl::make_unique<std::ofstream>(
-      std::string("timing_") + frame_id + ".csv", std::ios_base::out);
-
-  (*timing_file)
-      << "# Timing information for sensor with frame id: " << frame_id
-      << std::endl
-      << "# Columns are in order" << std::endl
-      << "# - packet index of the packet in the bag, first packet is 1"
-      << std::endl
-      << "# - timestamp when rosbag wrote the packet, i.e. "
-         "rosbag::MessageInstance::getTime().toNSec()"
-      << std::endl
-      << "# - timestamp when data was acquired, i.e. "
-         "message.header.stamp.toNSec()"
-      << std::endl
-      << "#" << std::endl
-      << "# The data can be read in python using" << std::endl
-      << "# import numpy" << std::endl
-      << "# np.loadtxt(<filename>, dtype='uint64')" << std::endl;
-
-  return timing_file;
-}
-
-void CheckImuMessage(const sensor_msgs::Imu& imu_message) {
-  auto linear_acceleration = ToEigen(imu_message.linear_acceleration);
-  if (std::isnan(linear_acceleration.norm()) ||
-      linear_acceleration.norm() < kMinLinearAcceleration ||
-      linear_acceleration.norm() > kMaxLinearAcceleration) {
-    LOG_FIRST_N(WARNING, 3)
-        << "frame_id " << imu_message.header.frame_id << " time "
-        << imu_message.header.stamp.toNSec() << ": IMU linear acceleration is "
-        << linear_acceleration.norm() << " m/s^2,"
-        << " expected is [" << kMinLinearAcceleration << ", "
-        << kMaxLinearAcceleration << "] m/s^2."
-        << " (It should include gravity and be given in m/s^2.)"
-        << " linear_acceleration " << linear_acceleration.transpose();
-  }
-}
-
-bool IsValidPose(const geometry_msgs::Pose& pose) {
-  return ToRigid3d(pose).IsValid();
-}
-
-void CheckOdometryMessage(const nav_msgs::Odometry& message) {
-  const auto& pose = message.pose.pose;
-  if (!IsValidPose(pose)) {
-    LOG_FIRST_N(ERROR, 3) << "frame_id " << message.header.frame_id << " time "
-                          << message.header.stamp.toNSec()
-                          << ": Odometry pose is invalid."
-                          << " pose " << pose;
-  }
-}
-
-void CheckTfMessage(const tf2_msgs::TFMessage& message) {
-  for (const auto& transform : message.transforms) {
-    if (transform.header.frame_id == "map") {
-      LOG_FIRST_N(ERROR, 1)
-          << "Input contains transform message from frame_id \""
-          << transform.header.frame_id << "\" to child_frame_id \""
-          << transform.child_frame_id
-          << "\". This is almost always output published by cartographer and "
-             "should not appear as input. (Unless you have some complex "
-             "remove_frames configuration, this is will not work. Simplest "
-             "solution is to record input without cartographer running.)";
-    }
-  }
-}
-
-bool IsPointDataType(const std::string& data_type) {
-  return (kPointDataTypes.count(data_type) != 0);
-}
-
-class RangeDataChecker {
- public:
-  template <typename MessageType>
-  void CheckMessage(const MessageType& message) {
-    const std::string& frame_id = message.header.frame_id;
-    ros::Time current_time_stamp = message.header.stamp;
-    RangeChecksum current_checksum;
-    cartographer::common::Time time_from, time_to;
-    ReadRangeMessage(message, &current_checksum, &time_from, &time_to);
-    auto previous_time_to_it = frame_id_to_previous_time_to_.find(frame_id);
-    if (previous_time_to_it != frame_id_to_previous_time_to_.end() &&
-        previous_time_to_it->second >= time_from) {
-      if (previous_time_to_it->second >= time_to) {
-        LOG_FIRST_N(ERROR, 3) << "Sensor with frame_id \"" << frame_id
-                              << "\" is not sequential in time."
-                              << "Previous range message ends at time "
-                              << previous_time_to_it->second
-                              << ", current one at time " << time_to;
-      } else {
-        LOG_FIRST_N(WARNING, 3)
-            << "Sensor with frame_id \"" << frame_id
-            << "\" measurements overlap in time. "
-            << "Previous range message, ending at time stamp "
-            << previous_time_to_it->second
-            << ", must finish before current range message, "
-            << "which ranges from " << time_from << " to " << time_to;
-      }
-      double overlap = cartographer::common::ToSeconds(
-          previous_time_to_it->second - time_from);
-      auto it = frame_id_to_max_overlap_duration_.find(frame_id);
-      if (it == frame_id_to_max_overlap_duration_.end() ||
-          overlap > frame_id_to_max_overlap_duration_.at(frame_id)) {
-        frame_id_to_max_overlap_duration_[frame_id] = overlap;
-      }
-    }
-    frame_id_to_previous_time_to_[frame_id] = time_to;
-    if (current_checksum.first == 0) {
-      return;
-    }
-    auto it = frame_id_to_range_checksum_.find(frame_id);
-    if (it != frame_id_to_range_checksum_.end()) {
-      RangeChecksum previous_checksum = it->second;
-      if (previous_checksum == current_checksum) {
-        LOG_FIRST_N(ERROR, 3)
-            << "Sensor with frame_id \"" << frame_id
-            << "\" sends exactly the same range measurements multiple times. "
-            << "Range data at time " << current_time_stamp
-            << " equals preceding data with " << current_checksum.first
-            << " points.";
-      }
-    }
-    frame_id_to_range_checksum_[frame_id] = current_checksum;
-  }
-
-  void PrintReport() {
-    for (auto& it : frame_id_to_max_overlap_duration_) {
-      LOG(WARNING) << "Sensor with frame_id \"" << it.first
-                   << "\" range measurements have longest overlap of "
-                   << it.second << " s";
-    }
-  }
-
- private:
-  typedef std::pair<size_t /* num_points */, Eigen::Vector4f /* points_sum */>
-      RangeChecksum;
-
-  template <typename MessageType>
-  static void ReadRangeMessage(const MessageType& message,
-                               RangeChecksum* range_checksum,
-                               cartographer::common::Time* from,
-                               cartographer::common::Time* to) {
-    auto point_cloud_time = ToPointCloudWithIntensities(message);
-    const cartographer::sensor::TimedPointCloud& point_cloud =
-        std::get<0>(point_cloud_time).points;
-    *to = std::get<1>(point_cloud_time);
-    *from = *to + cartographer::common::FromSeconds(point_cloud[0].time);
-    Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
-    for (const auto& point : point_cloud) {
-      points_sum.head<3>() += point.position;
-      points_sum[3] += point.time;
-    }
-    if (point_cloud.size() > 0) {
-      double first_point_relative_time = point_cloud[0].time;
-      double last_point_relative_time = (*point_cloud.rbegin()).time;
-      if (first_point_relative_time > 0) {
-        LOG_FIRST_N(ERROR, 1)
-            << "Sensor with frame_id \"" << message.header.frame_id
-            << "\" has range data that has positive relative time "
-            << first_point_relative_time << " s, must negative or zero.";
-      }
-      if (last_point_relative_time != 0) {
-        LOG_FIRST_N(INFO, 1)
-            << "Sensor with frame_id \"" << message.header.frame_id
-            << "\" has range data whose last point has relative time "
-            << last_point_relative_time << " s, should be zero.";
-      }
-    }
-    *range_checksum = {point_cloud.size(), points_sum};
-  }
-
-  std::map<std::string, RangeChecksum> frame_id_to_range_checksum_;
-  std::map<std::string, cartographer::common::Time>
-      frame_id_to_previous_time_to_;
-  std::map<std::string, double> frame_id_to_max_overlap_duration_;
-};
-
-void Run(const std::string& bag_filename, const bool dump_timing) {
-  rosbag::Bag bag;
-  bag.open(bag_filename, rosbag::bagmode::Read);
-  rosbag::View view(bag);
-
-  std::map<std::string, FrameProperties> frame_id_to_properties;
-  size_t message_index = 0;
-  int num_imu_messages = 0;
-  double sum_imu_acceleration = 0.;
-  RangeDataChecker range_data_checker;
-  for (const rosbag::MessageInstance& message : view) {
-    ++message_index;
-    std::string frame_id;
-    ros::Time time;
-    if (message.isType<sensor_msgs::PointCloud2>()) {
-      auto msg = message.instantiate<sensor_msgs::PointCloud2>();
-      time = msg->header.stamp;
-      frame_id = msg->header.frame_id;
-      range_data_checker.CheckMessage(*msg);
-    } else if (message.isType<sensor_msgs::MultiEchoLaserScan>()) {
-      auto msg = message.instantiate<sensor_msgs::MultiEchoLaserScan>();
-      time = msg->header.stamp;
-      frame_id = msg->header.frame_id;
-      range_data_checker.CheckMessage(*msg);
-    } else if (message.isType<sensor_msgs::LaserScan>()) {
-      auto msg = message.instantiate<sensor_msgs::LaserScan>();
-      time = msg->header.stamp;
-      frame_id = msg->header.frame_id;
-      range_data_checker.CheckMessage(*msg);
-    } else if (message.isType<sensor_msgs::Imu>()) {
-      auto msg = message.instantiate<sensor_msgs::Imu>();
-      time = msg->header.stamp;
-      frame_id = msg->header.frame_id;
-      CheckImuMessage(*msg);
-      num_imu_messages++;
-      sum_imu_acceleration += ToEigen(msg->linear_acceleration).norm();
-    } else if (message.isType<nav_msgs::Odometry>()) {
-      auto msg = message.instantiate<nav_msgs::Odometry>();
-      time = msg->header.stamp;
-      frame_id = msg->header.frame_id;
-      CheckOdometryMessage(*msg);
-    } else if (message.isType<tf2_msgs::TFMessage>()) {
-      auto msg = message.instantiate<tf2_msgs::TFMessage>();
-      CheckTfMessage(*msg);
-      continue;
-    } else {
-      continue;
-    }
-
-    bool first_packet = false;
-    if (!frame_id_to_properties.count(frame_id)) {
-      frame_id_to_properties.emplace(
-          frame_id,
-          FrameProperties{time, message.getTopic(), std::vector<float>(),
-                          dump_timing ? CreateTimingFile(frame_id) : nullptr,
-                          message.getDataType()});
-      first_packet = true;
-    }
-
-    auto& entry = frame_id_to_properties.at(frame_id);
-    if (!first_packet) {
-      const double delta_t_sec = (time - entry.last_timestamp).toSec();
-      if (delta_t_sec <= 0) {
-        LOG_FIRST_N(ERROR, 3)
-            << "Sensor with frame_id \"" << frame_id
-            << "\" jumps backwards in time, i.e. timestamps are not strictly "
-               "increasing. Make sure that the bag contains the data for each "
-               "frame_id sorted by header.stamp, i.e. the order in which they "
-               "were acquired from the sensor.";
-      }
-      entry.time_deltas.push_back(delta_t_sec);
-    }
-
-    if (entry.topic != message.getTopic()) {
-      LOG_FIRST_N(ERROR, 3)
-          << "frame_id \"" << frame_id
-          << "\" is send on multiple topics. It was seen at least on "
-          << entry.topic << " and " << message.getTopic();
-    }
-    entry.last_timestamp = time;
-
-    if (dump_timing) {
-      CHECK(entry.timing_file != nullptr);
-      (*entry.timing_file) << message_index << "\t"
-                           << message.getTime().toNSec() << "\t"
-                           << time.toNSec() << std::endl;
-    }
-
-    double duration_serialization_sensor = (time - message.getTime()).toSec();
-    if (std::abs(duration_serialization_sensor) >
-        kTimeDeltaSerializationSensorWarning) {
-      std::stringstream stream;
-      stream << "frame_id \"" << frame_id << "\" on topic "
-             << message.getTopic() << " has serialization time "
-             << message.getTime() << " but sensor time " << time
-             << " differing by " << duration_serialization_sensor << " s.";
-      if (std::abs(duration_serialization_sensor) >
-          kTimeDeltaSerializationSensorError) {
-        LOG_FIRST_N(ERROR, 3) << stream.str();
-      } else {
-        LOG_FIRST_N(WARNING, 1) << stream.str();
-      }
-    }
-  }
-  bag.close();
-
-  range_data_checker.PrintReport();
-
-  if (num_imu_messages > 0) {
-    double average_imu_acceleration = sum_imu_acceleration / num_imu_messages;
-    if (std::isnan(average_imu_acceleration) ||
-        average_imu_acceleration < kMinAverageAcceleration ||
-        average_imu_acceleration > kMaxAverageAcceleration) {
-      LOG(ERROR) << "Average IMU linear acceleration is "
-                 << average_imu_acceleration << " m/s^2,"
-                 << " expected is [" << kMinAverageAcceleration << ", "
-                 << kMaxAverageAcceleration
-                 << "] m/s^2. Linear acceleration data "
-                    "should include gravity and be given in m/s^2.";
-    }
-  }
-
-  constexpr int kNumBucketsForHistogram = 10;
-  for (const auto& entry_pair : frame_id_to_properties) {
-    const FrameProperties& frame_properties = entry_pair.second;
-    float max_time_delta =
-        *std::max_element(frame_properties.time_deltas.begin(),
-                          frame_properties.time_deltas.end());
-    if (IsPointDataType(frame_properties.data_type) &&
-        max_time_delta > kMaxGapPointsData) {
-      LOG(ERROR) << "Point data (frame_id: \"" << entry_pair.first
-                 << "\") has a large gap, largest is " << max_time_delta
-                 << " s, recommended is [0.0005, 0.05] s with no jitter.";
-    }
-    if (frame_properties.data_type ==
-            ros::message_traits::DataType<sensor_msgs::Imu>::value() &&
-        max_time_delta > kMaxGapImuData) {
-      LOG(ERROR) << "IMU data (frame_id: \"" << entry_pair.first
-                 << "\") has a large gap, largest is " << max_time_delta
-                 << " s, recommended is [0.0005, 0.005] s with no jitter.";
-    }
-
-    cartographer::common::Histogram histogram;
-    for (float time_delta : frame_properties.time_deltas) {
-      histogram.Add(time_delta);
-    }
-    LOG(INFO) << "Time delta histogram for consecutive messages on topic \""
-              << frame_properties.topic << "\" (frame_id: \""
-              << entry_pair.first << "\"):\n"
-              << histogram.ToString(kNumBucketsForHistogram);
-  }
-
-  if (dump_timing) {
-    for (const auto& entry_pair : frame_id_to_properties) {
-      entry_pair.second.timing_file->close();
-      CHECK(*entry_pair.second.timing_file)
-          << "Could not write timing information for \"" << entry_pair.first
-          << "\"";
-    }
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros
-
-int main(int argc, char** argv) {
-  FLAGS_alsologtostderr = true;
-  google::InitGoogleLogging(argv[0]);
-  google::ParseCommandLineFlags(&argc, &argv, true);
-
-  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
-  ::cartographer_ros::Run(FLAGS_bag_filename, FLAGS_dump_timing);
-}

cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -1,243 +0,0 @@
-/*
- * 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 "cartographer_ros/sensor_bridge.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/time_conversion.h"
-
-namespace cartographer_ros {
-
-namespace carto = ::cartographer;
-
-using carto::transform::Rigid3d;
-
-namespace {
-
-const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
-  if (frame_id.size() > 0) {
-    CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
-                               << " should not start with a /. See 1.7 in "
-                                  "http://wiki.ros.org/tf2/Migration.";
-  }
-  return frame_id;
-}
-
-}  // namespace
-
-SensorBridge::SensorBridge(
-    const int num_subdivisions_per_laser_scan,
-    const std::string& tracking_frame,
-    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
-    carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
-    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
-      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
-      trajectory_builder_(trajectory_builder) {}
-
-std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
-    const nav_msgs::Odometry::ConstPtr& msg) {
-  const carto::common::Time time = FromRos(msg->header.stamp);
-  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
-      time, CheckNoLeadingSlash(msg->child_frame_id));
-  if (sensor_to_tracking == nullptr) {
-    return nullptr;
-  }
-  return absl::make_unique<carto::sensor::OdometryData>(
-      carto::sensor::OdometryData{
-          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
-}
-
-void SensorBridge::HandleOdometryMessage(
-    const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
-  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
-      ToOdometryData(msg);
-  if (odometry_data != nullptr) {
-    trajectory_builder_->AddSensorData(
-        sensor_id,
-        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
-  }
-}
-
-void SensorBridge::HandleNavSatFixMessage(
-    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
-  const carto::common::Time time = FromRos(msg->header.stamp);
-  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
-    trajectory_builder_->AddSensorData(
-        sensor_id,
-        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
-    return;
-  }
-
-  if (!ecef_to_local_frame_.has_value()) {
-    ecef_to_local_frame_ =
-        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
-    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
-              << msg->latitude << ", long = " << msg->longitude << ".";
-  }
-
-  trajectory_builder_->AddSensorData(
-      sensor_id, carto::sensor::FixedFramePoseData{
-                     time, absl::optional<Rigid3d>(Rigid3d::Translation(
-                               ecef_to_local_frame_.value() *
-                               LatLongAltToEcef(msg->latitude, msg->longitude,
-                                                msg->altitude)))});
-}
-
-void SensorBridge::HandleLandmarkMessage(
-    const std::string& sensor_id,
-    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
-  auto landmark_data = ToLandmarkData(*msg);
-
-  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
-      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
-  if (tracking_from_landmark_sensor != nullptr) {
-    for (auto& observation : landmark_data.landmark_observations) {
-      observation.landmark_to_tracking_transform =
-          *tracking_from_landmark_sensor *
-          observation.landmark_to_tracking_transform;
-    }
-  }
-  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
-}
-
-std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
-    const sensor_msgs::Imu::ConstPtr& msg) {
-  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
-      << "Your IMU data claims to not contain linear acceleration measurements "
-         "by setting linear_acceleration_covariance[0] to -1. Cartographer "
-         "requires this data to work. See "
-         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
-  CHECK_NE(msg->angular_velocity_covariance[0], -1)
-      << "Your IMU data claims to not contain angular velocity measurements "
-         "by setting angular_velocity_covariance[0] to -1. Cartographer "
-         "requires this data to work. See "
-         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
-
-  const carto::common::Time time = FromRos(msg->header.stamp);
-  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
-      time, CheckNoLeadingSlash(msg->header.frame_id));
-  if (sensor_to_tracking == nullptr) {
-    return nullptr;
-  }
-  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
-      << "The IMU frame must be colocated with the tracking frame. "
-         "Transforming linear acceleration into the tracking frame will "
-         "otherwise be imprecise.";
-  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
-      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
-      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
-}
-
-void SensorBridge::HandleImuMessage(const std::string& sensor_id,
-                                    const sensor_msgs::Imu::ConstPtr& msg) {
-  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
-  if (imu_data != nullptr) {
-    trajectory_builder_->AddSensorData(
-        sensor_id,
-        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
-                               imu_data->angular_velocity});
-  }
-}
-
-void SensorBridge::HandleLaserScanMessage(
-    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
-  carto::sensor::PointCloudWithIntensities point_cloud;
-  carto::common::Time time;
-  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
-  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
-}
-
-void SensorBridge::HandleMultiEchoLaserScanMessage(
-    const std::string& sensor_id,
-    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  carto::sensor::PointCloudWithIntensities point_cloud;
-  carto::common::Time time;
-  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
-  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
-}
-
-void SensorBridge::HandlePointCloud2Message(
-    const std::string& sensor_id,
-    const sensor_msgs::PointCloud2::ConstPtr& msg) {
-  carto::sensor::PointCloudWithIntensities point_cloud;
-  carto::common::Time time;
-  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
-  HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
-}
-
-const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
-
-void SensorBridge::HandleLaserScan(
-    const std::string& sensor_id, const carto::common::Time time,
-    const std::string& frame_id,
-    const carto::sensor::PointCloudWithIntensities& points) {
-  if (points.points.empty()) {
-    return;
-  }
-  CHECK_LE(points.points.back().time, 0.f);
-  // TODO(gaschler): Use per-point time instead of subdivisions.
-  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
-    const size_t start_index =
-        points.points.size() * i / num_subdivisions_per_laser_scan_;
-    const size_t end_index =
-        points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
-    carto::sensor::TimedPointCloud subdivision(
-        points.points.begin() + start_index, points.points.begin() + end_index);
-    if (start_index == end_index) {
-      continue;
-    }
-    const double time_to_subdivision_end = subdivision.back().time;
-    // `subdivision_time` is the end of the measurement so sensor::Collator will
-    // send all other sensor data first.
-    const carto::common::Time subdivision_time =
-        time + carto::common::FromSeconds(time_to_subdivision_end);
-    auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
-    if (it != sensor_to_previous_subdivision_time_.end() &&
-        it->second >= subdivision_time) {
-      LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
-                   << sensor_id << " because previous subdivision time "
-                   << it->second << " is not before current subdivision time "
-                   << subdivision_time;
-      continue;
-    }
-    sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
-    for (auto& point : subdivision) {
-      point.time -= time_to_subdivision_end;
-    }
-    CHECK_EQ(subdivision.back().time, 0.f);
-    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
-  }
-}
-
-void SensorBridge::HandleRangefinder(
-    const std::string& sensor_id, const carto::common::Time time,
-    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
-  if (!ranges.empty()) {
-    CHECK_LE(ranges.back().time, 0.f);
-  }
-  const auto sensor_to_tracking =
-      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
-  if (sensor_to_tracking != nullptr) {
-    trajectory_builder_->AddSensorData(
-        sensor_id, carto::sensor::TimedPointCloudData{
-                       time, sensor_to_tracking->translation().cast<float>(),
-                       carto::sensor::TransformTimedPointCloud(
-                           ranges, sensor_to_tracking->cast<float>())});
-  }
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -1,99 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H
-
-#include <memory>
-
-#include "absl/types/optional.h"
-#include "cartographer/mapping/trajectory_builder_interface.h"
-#include "cartographer/sensor/imu_data.h"
-#include "cartographer/sensor/odometry_data.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/tf_bridge.h"
-#include "cartographer_ros_msgs/LandmarkList.h"
-#include "geometry_msgs/Transform.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "nav_msgs/OccupancyGrid.h"
-#include "nav_msgs/Odometry.h"
-#include "sensor_msgs/Imu.h"
-#include "sensor_msgs/LaserScan.h"
-#include "sensor_msgs/MultiEchoLaserScan.h"
-#include "sensor_msgs/NavSatFix.h"
-#include "sensor_msgs/PointCloud2.h"
-
-namespace cartographer_ros {
-
-// Converts ROS messages into SensorData in tracking frame for the MapBuilder.
-class SensorBridge {
- public:
-  explicit SensorBridge(
-      int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
-      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
-      ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
-
-  SensorBridge(const SensorBridge&) = delete;
-  SensorBridge& operator=(const SensorBridge&) = delete;
-
-  std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
-      const nav_msgs::Odometry::ConstPtr& msg);
-  void HandleOdometryMessage(const std::string& sensor_id,
-                             const nav_msgs::Odometry::ConstPtr& msg);
-  void HandleNavSatFixMessage(const std::string& sensor_id,
-                              const sensor_msgs::NavSatFix::ConstPtr& msg);
-  void HandleLandmarkMessage(
-      const std::string& sensor_id,
-      const cartographer_ros_msgs::LandmarkList::ConstPtr& msg);
-
-  std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
-      const sensor_msgs::Imu::ConstPtr& msg);
-  void HandleImuMessage(const std::string& sensor_id,
-                        const sensor_msgs::Imu::ConstPtr& msg);
-  void HandleLaserScanMessage(const std::string& sensor_id,
-                              const sensor_msgs::LaserScan::ConstPtr& msg);
-  void HandleMultiEchoLaserScanMessage(
-      const std::string& sensor_id,
-      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
-  void HandlePointCloud2Message(const std::string& sensor_id,
-                                const sensor_msgs::PointCloud2::ConstPtr& msg);
-
-  const TfBridge& tf_bridge() const;
-
- private:
-  void HandleLaserScan(
-      const std::string& sensor_id, ::cartographer::common::Time start_time,
-      const std::string& frame_id,
-      const ::cartographer::sensor::PointCloudWithIntensities& points);
-  void HandleRangefinder(const std::string& sensor_id,
-                         ::cartographer::common::Time time,
-                         const std::string& frame_id,
-                         const ::cartographer::sensor::TimedPointCloud& ranges);
-
-  const int num_subdivisions_per_laser_scan_;
-  std::map<std::string, cartographer::common::Time>
-      sensor_to_previous_subdivision_time_;
-  const TfBridge tf_bridge_;
-  ::cartographer::mapping::TrajectoryBuilderInterface* const
-      trajectory_builder_;
-
-  absl::optional<::cartographer::transform::Rigid3d> ecef_to_local_frame_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SENSOR_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/start_trajectory_main.cc

@@ -1,126 +0,0 @@
-/*
- * 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 <string>
-#include <vector>
-
-#include "absl/memory/memory.h"
-#include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/common/lua_parameter_dictionary.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/StatusCode.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.");
-
-DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");
-
-namespace cartographer_ros {
-namespace {
-
-TrajectoryOptions LoadOptions() {
-  auto file_resolver =
-      absl::make_unique<cartographer::common::ConfigurationFileResolver>(
-          std::vector<std::string>{FLAGS_configuration_directory});
-  const std::string code =
-      file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
-  auto lua_parameter_dictionary =
-      cartographer::common::LuaParameterDictionary::NonReferenceCounted(
-          code, std::move(file_resolver));
-  if (!FLAGS_initial_pose.empty()) {
-    auto initial_trajectory_pose_file_resolver =
-        absl::make_unique<cartographer::common::ConfigurationFileResolver>(
-            std::vector<std::string>{FLAGS_configuration_directory});
-    auto initial_trajectory_pose =
-        cartographer::common::LuaParameterDictionary::NonReferenceCounted(
-            "return " + FLAGS_initial_pose,
-            std::move(initial_trajectory_pose_file_resolver));
-    return CreateTrajectoryOptions(lua_parameter_dictionary.get(),
-                                   initial_trajectory_pose.get());
-  } else {
-    return CreateTrajectoryOptions(lua_parameter_dictionary.get());
-  }
-}
-
-bool Run() {
-  ros::NodeHandle node_handle;
-  ros::ServiceClient client =
-      node_handle.serviceClient<cartographer_ros_msgs::StartTrajectory>(
-          kStartTrajectoryServiceName);
-  cartographer_ros_msgs::StartTrajectory srv;
-  srv.request.options = ToRosMessage(LoadOptions());
-  srv.request.topics.laser_scan_topic = node_handle.resolveName(
-      kLaserScanTopic, true /* apply topic remapping */);
-  srv.request.topics.multi_echo_laser_scan_topic =
-      node_handle.resolveName(kMultiEchoLaserScanTopic, true);
-  srv.request.topics.point_cloud2_topic =
-      node_handle.resolveName(kPointCloud2Topic, true);
-  srv.request.topics.imu_topic = node_handle.resolveName(kImuTopic, true);
-  srv.request.topics.odometry_topic =
-      node_handle.resolveName(kOdometryTopic, true);
-
-  if (!client.call(srv)) {
-    LOG(ERROR) << "Failed to call " << kStartTrajectoryServiceName << ".";
-    return false;
-  }
-  if (srv.response.status.code != cartographer_ros_msgs::StatusCode::OK) {
-    LOG(ERROR) << "Error starting trajectory - message: '"
-               << srv.response.status.message
-               << "' (status code: " << std::to_string(srv.response.status.code)
-               << ").";
-    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;
-}

cartographer_ros/cartographer_ros/cartographer_ros/submap.cc

@@ -1,55 +0,0 @@
-/*
- * 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 "cartographer_ros/submap.h"
-
-#include "absl/memory/memory.h"
-#include "cartographer/common/port.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros_msgs/StatusCode.h"
-#include "cartographer_ros_msgs/SubmapQuery.h"
-
-namespace cartographer_ros {
-
-std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
-    const ::cartographer::mapping::SubmapId& submap_id,
-    ros::ServiceClient* client) {
-  ::cartographer_ros_msgs::SubmapQuery srv;
-  srv.request.trajectory_id = submap_id.trajectory_id;
-  srv.request.submap_index = submap_id.submap_index;
-  if (!client->call(srv) ||
-      srv.response.status.code != ::cartographer_ros_msgs::StatusCode::OK) {
-    return nullptr;
-  }
-  if (srv.response.textures.empty()) {
-    return nullptr;
-  }
-  auto response = absl::make_unique<::cartographer::io::SubmapTextures>();
-  response->version = srv.response.submap_version;
-  for (const auto& texture : srv.response.textures) {
-    const std::string compressed_cells(texture.cells.begin(),
-                                       texture.cells.end());
-    response->textures.emplace_back(::cartographer::io::SubmapTexture{
-        ::cartographer::io::UnpackTextureData(compressed_cells, texture.width,
-                                              texture.height),
-        texture.width, texture.height, texture.resolution,
-        ToRigid3d(texture.slice_pose)});
-  }
-  return response;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/submap.h

@@ -1,40 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_SUBMAP_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H
-
-#include <memory>
-#include <string>
-#include <vector>
-
-#include "cartographer/io/image.h"
-#include "cartographer/io/submap_painter.h"
-#include "cartographer/mapping/id.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "ros/ros.h"
-
-namespace cartographer_ros {
-
-// Fetch 'submap_id' using the 'client' and returning the response or 'nullptr'
-// on error.
-std::unique_ptr<::cartographer::io::SubmapTextures> FetchSubmapTextures(
-    const ::cartographer::mapping::SubmapId& submap_id,
-    ros::ServiceClient* client);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_SUBMAP_H

cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.cc

@@ -1,57 +0,0 @@
-/*
- * 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 "absl/memory/memory.h"
-
-#include "cartographer_ros/msg_conversion.h"
-#include "cartographer_ros/tf_bridge.h"
-
-namespace cartographer_ros {
-
-TfBridge::TfBridge(const std::string& tracking_frame,
-                   const double lookup_transform_timeout_sec,
-                   const tf2_ros::Buffer* buffer)
-    : tracking_frame_(tracking_frame),
-      lookup_transform_timeout_sec_(lookup_transform_timeout_sec),
-      buffer_(buffer) {}
-
-std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking(
-    const ::cartographer::common::Time time,
-    const std::string& frame_id) const {
-  ::ros::Duration timeout(lookup_transform_timeout_sec_);
-  std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
-  try {
-    const ::ros::Time latest_tf_time =
-        buffer_
-            ->lookupTransform(tracking_frame_, frame_id, ::ros::Time(0.),
-                              timeout)
-            .header.stamp;
-    const ::ros::Time requested_time = ToRos(time);
-    if (latest_tf_time >= requested_time) {
-      // We already have newer data, so we do not wait. Otherwise, we would wait
-      // for the full 'timeout' even if we ask for data that is too old.
-      timeout = ::ros::Duration(0.);
-    }
-    return absl::make_unique<::cartographer::transform::Rigid3d>(
-        ToRigid3d(buffer_->lookupTransform(tracking_frame_, frame_id,
-                                           requested_time, timeout)));
-  } catch (const tf2::TransformException& ex) {
-    LOG(WARNING) << ex.what();
-  }
-  return nullptr;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/tf_bridge.h

@@ -1,51 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_TF_BRIDGE_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H
-
-#include <memory>
-
-#include "cartographer/transform/rigid_transform.h"
-#include "tf2_ros/buffer.h"
-
-#include "cartographer_ros/time_conversion.h"
-
-namespace cartographer_ros {
-
-class TfBridge {
- public:
-  TfBridge(const std::string& tracking_frame,
-           double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
-  ~TfBridge() {}
-
-  TfBridge(const TfBridge&) = delete;
-  TfBridge& operator=(const TfBridge&) = delete;
-
-  // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
-  // 'time'.
-  std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking(
-      ::cartographer::common::Time time, const std::string& frame_id) const;
-
- private:
-  const std::string tracking_frame_;
-  const double lookup_transform_timeout_sec_;
-  const tf2_ros::Buffer* const buffer_;
-};
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TF_BRIDGE_H

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion.cc

@@ -1,47 +0,0 @@
-/*
- * 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 "cartographer_ros/time_conversion.h"
-
-#include "cartographer/common/time.h"
-#include "ros/ros.h"
-
-namespace cartographer_ros {
-
-::ros::Time ToRos(::cartographer::common::Time time) {
-  int64_t uts_timestamp = ::cartographer::common::ToUniversal(time);
-  int64_t ns_since_unix_epoch =
-      (uts_timestamp -
-       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
-           10000000ll) *
-      100ll;
-  ::ros::Time ros_time;
-  ros_time.fromNSec(ns_since_unix_epoch);
-  return ros_time;
-}
-
-// TODO(pedrofernandez): Write test.
-::cartographer::common::Time FromRos(const ::ros::Time& time) {
-  // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
-  // exactly 719162 days before the Unix epoch.
-  return ::cartographer::common::FromUniversal(
-      (time.sec +
-       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) *
-          10000000ll +
-      (time.nsec + 50) / 100);  // + 50 to get the rounding correct.
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion.h

@@ -1,31 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_TIME_CONVERSION_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H
-
-#include "cartographer/common/time.h"
-#include "ros/ros.h"
-
-namespace cartographer_ros {
-
-::ros::Time ToRos(::cartographer::common::Time time);
-
-::cartographer::common::Time FromRos(const ::ros::Time& time);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TIME_CONVERSION_H

cartographer_ros/cartographer_ros/cartographer_ros/time_conversion_test.cc

@@ -1,44 +0,0 @@
-/*
- * 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 "cartographer_ros/time_conversion.h"
-
-#include <chrono>
-
-#include "cartographer/common/time.h"
-#include "gtest/gtest.h"
-#include "ros/ros.h"
-
-namespace cartographer_ros {
-namespace {
-
-TEST(TimeConversion, testToRos) {
-  std::vector<int64_t> values = {0, 1469091375, 1466481821, 1462101382,
-                                 1468238899};
-  for (int64_t seconds_since_epoch : values) {
-    ::ros::Time ros_now;
-    ros_now.fromSec(seconds_since_epoch);
-    ::cartographer::common::Time cartographer_now(
-        ::cartographer::common::FromSeconds(
-            seconds_since_epoch +
-            ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds));
-    EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now));
-    EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now));
-  }
-}
-
-}  // namespace
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -1,163 +0,0 @@
-/*
- * 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 "cartographer_ros/trajectory_options.h"
-
-#include "cartographer/mapping/trajectory_builder_interface.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/transform.h"
-#include "cartographer_ros/time_conversion.h"
-#include "glog/logging.h"
-
-namespace cartographer_ros {
-
-namespace {
-
-void CheckTrajectoryOptions(const TrajectoryOptions& options) {
-  CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
-  CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans +
-               options.num_point_clouds,
-           1)
-      << "Configuration error: 'num_laser_scans', "
-         "'num_multi_echo_laser_scans' and 'num_point_clouds' are "
-         "all zero, but at least one is required.";
-}
-
-}  // namespace
-
-TrajectoryOptions CreateTrajectoryOptions(
-    ::cartographer::common::LuaParameterDictionary* const
-        lua_parameter_dictionary) {
-  TrajectoryOptions options;
-  options.trajectory_builder_options =
-      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
-          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
-  options.tracking_frame =
-      lua_parameter_dictionary->GetString("tracking_frame");
-  options.published_frame =
-      lua_parameter_dictionary->GetString("published_frame");
-  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
-  options.provide_odom_frame =
-      lua_parameter_dictionary->GetBool("provide_odom_frame");
-  options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
-  options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat");
-  options.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks");
-  options.publish_frame_projected_to_2d =
-      lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
-  options.num_laser_scans =
-      lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
-  options.num_multi_echo_laser_scans =
-      lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
-  options.num_subdivisions_per_laser_scan =
-      lua_parameter_dictionary->GetNonNegativeInt(
-          "num_subdivisions_per_laser_scan");
-  options.num_point_clouds =
-      lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
-  options.rangefinder_sampling_ratio =
-      lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio");
-  options.odometry_sampling_ratio =
-      lua_parameter_dictionary->GetDouble("odometry_sampling_ratio");
-  options.fixed_frame_pose_sampling_ratio =
-      lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
-  options.imu_sampling_ratio =
-      lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
-  options.landmarks_sampling_ratio =
-      lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
-  CheckTrajectoryOptions(options);
-  return options;
-}
-
-TrajectoryOptions CreateTrajectoryOptions(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
-    ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose) {
-  TrajectoryOptions options = CreateTrajectoryOptions(lua_parameter_dictionary);
-  *options.trajectory_builder_options.mutable_initial_trajectory_pose() =
-      CreateInitialTrajectoryPose(initial_trajectory_pose);
-  return options;
-}
-
-::cartographer::mapping::proto::InitialTrajectoryPose
-CreateInitialTrajectoryPose(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary) {
-  ::cartographer::mapping::proto::InitialTrajectoryPose pose;
-  pose.set_to_trajectory_id(
-      lua_parameter_dictionary->GetNonNegativeInt("to_trajectory_id"));
-  *pose.mutable_relative_pose() =
-      cartographer::transform::ToProto(cartographer::transform::FromDictionary(
-          lua_parameter_dictionary->GetDictionary("relative_pose").get()));
-  pose.set_timestamp(
-      lua_parameter_dictionary->HasKey("timestamp")
-          ? lua_parameter_dictionary->GetNonNegativeInt("timestamp")
-          : cartographer::common::ToUniversal(FromRos(ros::Time::now())));
-  return pose;
-}
-
-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_nav_sat = msg.use_nav_sat;
-  options->use_landmarks = msg.use_landmarks;
-  options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d;
-  options->num_laser_scans = msg.num_laser_scans;
-  options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
-  options->num_subdivisions_per_laser_scan =
-      msg.num_subdivisions_per_laser_scan;
-  options->num_point_clouds = msg.num_point_clouds;
-  options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio;
-  options->odometry_sampling_ratio = msg.odometry_sampling_ratio;
-  options->fixed_frame_pose_sampling_ratio =
-      msg.fixed_frame_pose_sampling_ratio;
-  options->imu_sampling_ratio = msg.imu_sampling_ratio;
-  options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio;
-  if (!options->trajectory_builder_options.ParseFromString(
-          msg.trajectory_builder_options_proto)) {
-    LOG(ERROR) << "Failed to parse protobuf";
-    return false;
-  }
-  CheckTrajectoryOptions(*options);
-  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_nav_sat = options.use_nav_sat;
-  msg.use_landmarks = options.use_landmarks;
-  msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d;
-  msg.num_laser_scans = options.num_laser_scans;
-  msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
-  msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
-  msg.num_point_clouds = options.num_point_clouds;
-  msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio;
-  msg.odometry_sampling_ratio = options.odometry_sampling_ratio;
-  msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio;
-  msg.imu_sampling_ratio = options.imu_sampling_ratio;
-  msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio;
-  options.trajectory_builder_options.SerializeToString(
-      &msg.trajectory_builder_options_proto);
-  return msg;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/trajectory_options.h

@@ -1,72 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H
-
-#include <string>
-
-#include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/port.h"
-#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
-#include "cartographer_ros_msgs/TrajectoryOptions.h"
-
-namespace cartographer_ros {
-
-struct TrajectoryOptions {
-  ::cartographer::mapping::proto::TrajectoryBuilderOptions
-      trajectory_builder_options;
-  std::string tracking_frame;
-  std::string published_frame;
-  std::string odom_frame;
-  bool provide_odom_frame;
-  bool use_odometry;
-  bool use_nav_sat;
-  bool use_landmarks;
-  bool publish_frame_projected_to_2d;
-  int num_laser_scans;
-  int num_multi_echo_laser_scans;
-  int num_subdivisions_per_laser_scan;
-  int num_point_clouds;
-  double rangefinder_sampling_ratio;
-  double odometry_sampling_ratio;
-  double fixed_frame_pose_sampling_ratio;
-  double imu_sampling_ratio;
-  double landmarks_sampling_ratio;
-};
-
-::cartographer::mapping::proto::InitialTrajectoryPose
-CreateInitialTrajectoryPose(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
-
-TrajectoryOptions CreateTrajectoryOptions(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
-
-TrajectoryOptions CreateTrajectoryOptions(
-    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary,
-    ::cartographer::common::LuaParameterDictionary* initial_trajectory_pose);
-
-// 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_CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H

cartographer_ros/cartographer_ros/cartographer_ros/urdf_reader.cc

@@ -1,59 +0,0 @@
-/*
- * 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 "cartographer_ros/urdf_reader.h"
-
-#include <string>
-#include <vector>
-
-#include "cartographer_ros/msg_conversion.h"
-#include "urdf/model.h"
-
-namespace cartographer_ros {
-
-std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
-    const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) {
-  urdf::Model model;
-  CHECK(model.initFile(urdf_filename));
-#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS
-  std::vector<urdf::LinkSharedPtr> links;
-#else
-  std::vector<boost::shared_ptr<urdf::Link> > links;
-#endif
-  model.getLinks(links);
-  std::vector<geometry_msgs::TransformStamped> transforms;
-  for (const auto& link : links) {
-    if (!link->getParent() || link->parent_joint->type != urdf::Joint::FIXED) {
-      continue;
-    }
-
-    const urdf::Pose& pose =
-        link->parent_joint->parent_to_joint_origin_transform;
-    geometry_msgs::TransformStamped transform;
-    transform.transform =
-        ToGeometryMsgTransform(cartographer::transform::Rigid3d(
-            Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z),
-            Eigen::Quaterniond(pose.rotation.w, pose.rotation.x,
-                               pose.rotation.y, pose.rotation.z)));
-    transform.child_frame_id = link->name;
-    transform.header.frame_id = link->getParent()->name;
-    tf_buffer->setTransform(transform, "urdf", true /* is_static */);
-    transforms.push_back(transform);
-  }
-  return transforms;
-}
-
-}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/cartographer_ros/urdf_reader.h

@@ -1,32 +0,0 @@
-/*
- * 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_CARTOGRAPHER_ROS_URDF_READER_H
-#define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H
-
-#include <vector>
-
-#include "cartographer/common/port.h"
-#include "tf2_ros/buffer.h"
-
-namespace cartographer_ros {
-
-std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
-    const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer);
-
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_URDF_READER_H

cartographer_ros/cartographer_ros/configuration_files/assets_writer_backpack_2d.lua

@@ -1,131 +0,0 @@
---
---
---
-
-VOXEL_SIZE = 5e-2
-
-include "transform.lua"
-
-options = {
-  tracking_frame = "base_link",
-  pipeline = {
-    {
-      action = "min_max_range_filter",
-      min_range = 1.,
-      max_range = 60.,
-    },
-    {
-      action = "dump_num_points",
-    },
-
-    -- Gray X-Rays. These only use geometry to color pixels.
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_yz_all",
-      transform = YZ_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xy_all",
-      transform = XY_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xz_all",
-      transform = XZ_TRANSFORM,
-    },
-
-    -- We now use the intensities to color our points. We apply a linear
-    -- transform to clamp our intensity values into [0, 255] and then use this
-    -- value for RGB of our points. Every stage in the pipeline after this now
-    -- receives colored points.
-    --
-    -- We write xrays again. These now use geometry and the intensities to
-    -- color pixels - they look quite similar, just a little lighter.
-    {
-      action = "intensity_to_color",
-      min_intensity = 0.,
-      max_intensity = 4095.,
-    },
-
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_yz_all_intensity",
-      transform = YZ_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xy_all_intensity",
-      transform = XY_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xz_all_intensity",
-      transform = XZ_TRANSFORM,
-    },
-
-    -- We also write a PLY file at this stage, because gray points look good.
-    -- The points in the PLY can be visualized using
-    -- https://github.com/googlecartographer/point_cloud_viewer.
-    {
-      action = "write_ply",
-      filename = "points.ply",
-    },
-
-    -- Now we recolor our points by frame and write another batch of X-Rays. It
-    -- is visible in them what was seen by the horizontal and the vertical
-    -- laser.
-    {
-      action = "color_points",
-      frame_id = "horizontal_laser_link",
-      color = { 255., 0., 0. },
-    },
-    {
-      action = "color_points",
-      frame_id = "vertical_laser_link",
-      color = { 0., 255., 0. },
-    },
-
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_yz_all_color",
-      transform = YZ_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xy_all_color",
-      transform = XY_TRANSFORM,
-    },
-    {
-      action = "write_xray_image",
-      voxel_size = VOXEL_SIZE,
-      filename = "xray_xz_all_color",
-      transform = XZ_TRANSFORM,
-    },
-  }
-}
-
-return options