add first ros2 project, not complete.

src/map/odtolanelet/main.cpp

@@ -10,10 +10,10 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    odtolanelet * potl = new odtolanelet("/home/yuchuli/map/mapxiali.xodr");
+    odtolanelet * potl = new odtolanelet("/home/yuchuli/line.xodr");
 
     int64_t time1 = std::chrono::system_clock::now().time_since_epoch().count();
-    potl->ConvertToLanelet("/home/yuchuli/testlane.osm");
+    potl->ConvertToLanelet("/home/yuchuli/line.osm");
     int64_t time2 = std::chrono::system_clock::now().time_since_epoch().count();
 
     std::cout<<"use time: "<<(time2 - time1)<<std::endl;

src/ros2/src/testpose/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.14)
+project(testpose)
+
+#Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+
+include_directories(
+##  INCLUDE_DIRS include
+  ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${catkin_INCLUDE_DIRS} include
+)
+
+
+add_executable(testpose
+  src/testpose_node.cpp
+  src/testpose_core.cpp
+)
+
+ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs tf2_geometry_msgs)
+
+install(TARGETS
+testpose
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()
+
+

src/ros2/src/testpose/include/testpose/testpose_core.hpp

@@ -0,0 +1,38 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// 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 TESTPOSE_TESTPOSE_CORE_HPP_
+#define TESTPOSE__TESTPOSE_CORE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+
+class TestPose : public rclcpp::Node
+{
+public:
+  TestPose();
+  ~TestPose() = default;
+
+private:
+  void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
+
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+
+};
+
+#endif  // TESTPOSE__TESTPOSE_CORE_HPP_

src/ros2/src/testpose/launch/testpose.xml

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="input_pose_topic" default="/localization/pose_estimator/pose" description=""/>
+  <arg name="output_twist_topic" default="/estimate_twist" description=""/>
+
+  <node pkg="testpose" exec="testpose" name="testpose" output="log">
+    <remap from="pose" to="$(var input_pose_topic)"/>
+    <remap from="twist" to="$(var output_twist_topic)"/>
+  </node>
+</launch>

src/ros2/src/testpose/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>testpose</name>
+  <version>0.1.0</version>
+  <description>testpose</description>
+  <maintainer email="yuchuli@catarc.ac.cn">Yu Chuli</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yuchuli@catarc.ac.cn">Yu Chuli</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+
+
+</package>

src/ros2/src/testpose/src/testpose_core.cpp

@@ -0,0 +1,113 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// 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 "testpose/testpose_core.hpp"
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <cmath>
+#include <cstddef>
+#include <functional>
+
+TestPose::TestPose() : Node("testpose_core")
+{
+  using std::placeholders::_1;
+
+  static constexpr std::size_t queue_size = 1;
+  rclcpp::QoS durable_qos(queue_size);
+  durable_qos.transient_local();
+
+  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", durable_qos);
+  // Note: this callback publishes topics above
+  pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
+    "pose", queue_size, std::bind(&TestPose::callbackPose, this, _1));
+}
+
+double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+  double diff_rad = lhs_rad - rhs_rad;
+  if (diff_rad > M_PI) {
+    diff_rad = diff_rad - 2 * M_PI;
+  } else if (diff_rad < -M_PI) {
+    diff_rad = diff_rad + 2 * M_PI;
+  }
+  return diff_rad;
+}
+
+// x: roll, y: pitch, z: yaw
+geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose)
+{
+  geometry_msgs::msg::Vector3 rpy;
+  tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
+  tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z);
+  return rpy;
+}
+
+geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose)
+{
+  return getRPY(pose->pose);
+}
+
+geometry_msgs::msg::TwistStamped calcTwist(
+  geometry_msgs::msg::PoseStamped::SharedPtr pose_a,
+  geometry_msgs::msg::PoseStamped::SharedPtr pose_b)
+{
+  const double dt =
+    (rclcpp::Time(pose_b->header.stamp) - rclcpp::Time(pose_a->header.stamp)).seconds();
+
+  if (dt == 0) {
+    geometry_msgs::msg::TwistStamped twist;
+    twist.header = pose_b->header;
+    return twist;
+  }
+
+  const auto pose_a_rpy = getRPY(pose_a);
+  const auto pose_b_rpy = getRPY(pose_b);
+
+  geometry_msgs::msg::Vector3 diff_xyz;
+  geometry_msgs::msg::Vector3 diff_rpy;
+
+  diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x;
+  diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y;
+  diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z;
+  diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x);
+  diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y);
+  diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z);
+
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header = pose_b->header;
+  twist.twist.linear.x =
+    std::sqrt(std::pow(diff_xyz.x, 2.0) + std::pow(diff_xyz.y, 2.0) + std::pow(diff_xyz.z, 2.0)) /
+    dt;
+  twist.twist.linear.y = 0;
+  twist.twist.linear.z = 0;
+  twist.twist.angular.x = diff_rpy.x / dt;
+  twist.twist.angular.y = diff_rpy.y / dt;
+  twist.twist.angular.z = diff_rpy.z / dt;
+
+  return twist;
+}
+
+void TestPose::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
+{
+  // TODO(YamatoAndo) check time stamp diff
+  // TODO(YamatoAndo) check suddenly move
+  // TODO(YamatoAndo) apply low pass filter
+
+
+}

src/ros2/src/testpose/src/testpose_node.cpp

@@ -0,0 +1,29 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// 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 "testpose/testpose_core.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<TestPose>());
+  rclcpp::shutdown();
+
+  return 0;
+}