Skip to content

Instantly share code, notes, and snippets.

@gyubeomim
Last active January 29, 2019 08:27
Show Gist options
  • Save gyubeomim/7f67a9ca10824ea56875ad4b7a449b49 to your computer and use it in GitHub Desktop.
Save gyubeomim/7f67a9ca10824ea56875ad4b7a449b49 to your computer and use it in GitHub Desktop.
modified start_trajectory_main.cc file to change starting point at initial time
/*
* 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/common/lua_parameter_dictionary.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/port.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/trajectory_options.h"
#include "cartographer_ros_msgs/StartTrajectory.h"
#include "cartographer_ros_msgs/TrajectoryOptions.h"
#include "gflags/gflags.h"
#include "ros/ros.h"
// ed: header added
#include <chrono>
#include <thread>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
#include <tf2/utils.h>
#include "cartographer/io/proto_stream.h"
#include "cartographer_ros/submap.h"
#include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
using namespace std;
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");
// ed: pbstream filename to get last trajectory's position(x,y,z) value
DEFINE_string(pbstream_filename, "",
"Filename of a pbstream to draw a map from.");
namespace cartographer_ros {
namespace {
TrajectoryOptions LoadOptions() {
auto file_resolver = cartographer::common::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));
// ed: if there is initial_pose value
if (!FLAGS_initial_pose.empty()) {
auto initial_trajectory_pose_file_resolver =
cartographer::common::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());
// ed: code added
/* std::cout << srv.request.options << std::endl; */
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) << "Error starting trajectory.";
return false;
}
LOG(INFO) << "Started trajectory " << srv.response.trajectory_id;
return true;
}
} // namespace
} // namespace cartographer_ros
// ed: /init_pose_string subscribe callback function
void init_pose_callback(const std_msgs::String::ConstPtr& msg){
std::cout << "[+] CB : /init_pose_string data received! " << std::endl;
FLAGS_initial_pose = msg->data;
}
// ed: /move_base_simple/goal (2D Nav Goal in Rviz) subscribe callback function
void move_base_simple_callback(const geometry_msgs::PoseStamped::ConstPtr& msg){
std::cout << "[+] CB : /move_base_simple/goal data received! " << std::endl;
::cartographer::io::ProtoStreamReader reader(FLAGS_pbstream_filename);
cartographer_ros::NodeOptions node_options;
cartographer_ros::TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
cartographer_ros::LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
auto map_builder =
cartographer::common::make_unique<cartographer::mapping::MapBuilder>(
node_options.map_builder_options);
// ed: load pbstream
map_builder->LoadState(&reader, true);
// ed: get TrajectoryNodePose
const auto node_poses = map_builder->pose_graph()->GetTrajectoryNodePoses();
double pbstream_x = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().x();
double pbstream_y = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().y();
double pbstream_z = std::prev(node_poses.EndOfTrajectory(0))->data.global_pose.translation().z();
double pbstream_quat_x = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().x();
double pbstream_quat_y = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().y();
double pbstream_quat_z = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().z();
double pbstream_quat_w = (std::prev(node_poses.EndOfTrajectory(0)))->data.global_pose.rotation().w();
tf::Transform map_tf;
tf::Transform EOT_tf;
tf::Transform output_tf;
map_tf.setOrigin(tf::Vector3(msg->pose.position.x,
msg->pose.position.y,
msg->pose.position.z));
map_tf.setRotation(tf::Quaternion(msg->pose.orientation.x,
msg->pose.orientation.y,
msg->pose.orientation.z,
msg->pose.orientation.w));
EOT_tf.setOrigin(tf::Vector3(pbstream_x,
pbstream_y,
pbstream_z));
EOT_tf.setRotation(tf::Quaternion(pbstream_quat_x,
pbstream_quat_y,
pbstream_quat_z,
pbstream_quat_w));
output_tf = EOT_tf * map_tf;
// ed: Quaternion to RPY
tf::Quaternion q(
output_tf.getRotation().x(),
output_tf.getRotation().y(),
output_tf.getRotation().z(),
output_tf.getRotation().w());
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
int to_trajectory_id = 0; // ed: offline map's trajectory
std::cout << "[+] map_tf : ["
<< map_tf.getOrigin().x() << ", "
<< map_tf.getOrigin().y() << ", "
<< map_tf.getOrigin().z() << "] " << std::endl;
std::cout << "[+] EOT_tf : ["
<< EOT_tf.getOrigin().x() << ", "
<< EOT_tf.getOrigin().y() << ", "
<< EOT_tf.getOrigin().z() << "] " << std::endl;
std::cout << "[+] output_tf : ["
<< output_tf.getOrigin().x() << ", "
<< output_tf.getOrigin().y() << ", "
<< output_tf.getOrigin().z() << "] " << std::endl;
// ed: pitch isn't used
std::string parsed_string ="{to_trajectory_id = " \
+ std::to_string(to_trajectory_id) \
+ ", relative_pose = { translation = { " \
+ std::to_string(output_tf.getOrigin().x()) + ", " \
+ std::to_string(output_tf.getOrigin().y()) + ", " \
+ "0" + \
+ "}, rotation = { "\
+ std::to_string(roll) + "," \
+ std::to_string(0) + "," \
+ std::to_string(yaw) + "}}}";
// ed: insert value into FLAGS_initial_pose string
FLAGS_initial_pose = parsed_string;
}
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();
// ed: code added
::ros::NodeHandle nh;
::ros::Subscriber sub_init_pose = nh.subscribe<std_msgs::String>("/init_pose_string", 1, ::init_pose_callback);
::ros::Subscriber sub_move_base_simple = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &move_base_simple_callback);
::ros::Rate loop_rate(1);
// ed: while loop until /init_pose_string
while(::ros::ok()) {
if(!FLAGS_initial_pose.empty()){
sub_init_pose.shutdown();
std::cout << "data received : " << FLAGS_initial_pose << std::endl;
cartographer_ros::ScopedRosLogSink ros_log_sink;
int exit_code = cartographer_ros::Run() ? 0 : 1;
::ros::shutdown();
return exit_code;
}
std::cout << "waiting for data..." << std::endl;
::ros::spinOnce();
loop_rate.sleep();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment