|
|
@@ -0,0 +1,411 @@
|
|
|
+// 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 "pilot_autoware_bridge/pilot_autoware_bridge_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>
|
|
|
+
|
|
|
+#include <time.h>
|
|
|
+#include <sstream>
|
|
|
+
|
|
|
+#include <iostream>
|
|
|
+
|
|
|
+#include <GeographicLib/MGRS.hpp>
|
|
|
+#include <GeographicLib/UTMUPS.hpp>
|
|
|
+
|
|
|
+#include <QString>
|
|
|
+
|
|
|
+#include "modulecomm.h"
|
|
|
+
|
|
|
+using namespace std;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+pilot_autoware_bridge::pilot_autoware_bridge() : Node("testpose_core")
|
|
|
+{
|
|
|
+ QString str;
|
|
|
+ using std::placeholders::_1;
|
|
|
+
|
|
|
+ simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
|
|
|
+ origin_frame_id_ = declare_parameter("origin_frame_id", "map");
|
|
|
+
|
|
|
+ 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(&pilot_autoware_bridge::callbackPose, this, _1));
|
|
|
+
|
|
|
+pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
|
|
|
+pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
|
|
|
+
|
|
|
+pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
|
|
|
+
|
|
|
+pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", durable_qos);
|
|
|
+pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
|
|
|
+
|
|
|
+ mptimer= create_wall_timer(100ms,std::bind(&pilot_autoware_bridge::callbackTimer, this));
|
|
|
+
|
|
|
+ ModuleFun fungpsimu =std::bind(&pilot_autoware_bridge::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
|
|
|
+ mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+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;
|
|
|
+}
|
|
|
+
|
|
|
+struct Quaternion {
|
|
|
+ double w, x, y, z;
|
|
|
+};
|
|
|
+
|
|
|
+struct EulerAngles {
|
|
|
+ double roll, pitch, yaw;
|
|
|
+};
|
|
|
+
|
|
|
+EulerAngles ToEulerAngles(Quaternion q) {
|
|
|
+ EulerAngles angles;
|
|
|
+
|
|
|
+ // roll (x-axis rotation)
|
|
|
+ double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
|
|
|
+ double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
|
|
|
+ angles.roll = std::atan2(sinr_cosp, cosr_cosp);
|
|
|
+
|
|
|
+ // pitch (y-axis rotation)
|
|
|
+ double sinp = 2 * (q.w * q.y - q.z * q.x);
|
|
|
+ if (std::abs(sinp) >= 1)
|
|
|
+ angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
|
|
|
+ else
|
|
|
+ angles.pitch = std::asin(sinp);
|
|
|
+
|
|
|
+ // yaw (z-axis rotation)
|
|
|
+ double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
|
|
|
+ double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
|
|
|
+ angles.yaw = std::atan2(siny_cosp, cosy_cosp);
|
|
|
+
|
|
|
+ return angles;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
|
|
|
+{
|
|
|
+ // Abbreviations for the various angular functions
|
|
|
+ double cy = cos(yaw * 0.5);
|
|
|
+ double sy = sin(yaw * 0.5);
|
|
|
+ double cp = cos(pitch * 0.5);
|
|
|
+ double sp = sin(pitch * 0.5);
|
|
|
+ double cr = cos(roll * 0.5);
|
|
|
+ double sr = sin(roll * 0.5);
|
|
|
+
|
|
|
+ Quaternion q;
|
|
|
+ q.w = cy * cp * cr + sy * sp * sr;
|
|
|
+ q.x = cy * cp * sr - sy * sp * cr;
|
|
|
+ q.y = sy * cp * sr + cy * sp * cr;
|
|
|
+ q.z = sy * cp * cr - cy * sp * sr;
|
|
|
+
|
|
|
+ return q;
|
|
|
+}
|
|
|
+
|
|
|
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
|
|
|
+
|
|
|
+void pilot_autoware_bridge::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
|
|
|
+{
|
|
|
+ (void)pose_msg_ptr;
|
|
|
+ // TODO(YamatoAndo) check time stamp diff
|
|
|
+ // TODO(YamatoAndo) check suddenly move
|
|
|
+ // TODO(YamatoAndo) apply low pass filter
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::callbackTimer()
|
|
|
+{
|
|
|
+ std::cout<<" testpose. "<<std::endl;
|
|
|
+
|
|
|
+ double pitch,roll,yaw;
|
|
|
+ pitch = 0;
|
|
|
+ roll = 0;
|
|
|
+
|
|
|
+ yaw = 0;
|
|
|
+ Quaternion quat = ToQuaternion(yaw,pitch,roll);
|
|
|
+
|
|
|
+ nav_msgs::msg::Odometry msg;
|
|
|
+
|
|
|
+ double flon,flat;
|
|
|
+ flon = 117.0857970;
|
|
|
+ flat = 39.1366668;
|
|
|
+
|
|
|
+ double fx,fy;
|
|
|
+ CalcXY(flon,flat,fx,fy);
|
|
|
+
|
|
|
+ static double rel_x = fx;//7600;
|
|
|
+ static double rel_y = fy;//32100;
|
|
|
+ static double rel_z = 0;
|
|
|
+ static double vn = 0;
|
|
|
+ static double ve = 1;
|
|
|
+ // rel_x+=0.01;
|
|
|
+
|
|
|
+ msg.pose.pose.position.x = rel_x;
|
|
|
+ msg.pose.pose.position.y = rel_y;
|
|
|
+ msg.pose.pose.position.z = rel_z;
|
|
|
+
|
|
|
+ msg.pose.pose.orientation.x = quat.x;
|
|
|
+ msg.pose.pose.orientation.y = quat.y;
|
|
|
+ msg.pose.pose.orientation.z = quat.z;
|
|
|
+ msg.pose.pose.orientation.w = quat.w;
|
|
|
+
|
|
|
+ msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
|
|
|
+
|
|
|
+ publish_odometry(msg);
|
|
|
+ publish_tf(msg);
|
|
|
+
|
|
|
+ autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
|
|
|
+ velocity.longitudinal_velocity = 0;
|
|
|
+ velocity.lateral_velocity = 0.0F;
|
|
|
+ velocity.heading_rate = 0;
|
|
|
+ publish_velocity(velocity);
|
|
|
+
|
|
|
+ current_steer_.steering_tire_angle = static_cast<double>(0);
|
|
|
+ current_steer_.stamp = get_clock()->now();
|
|
|
+
|
|
|
+ publish_steering(current_steer_);
|
|
|
+
|
|
|
+ publish_acceleration();
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::publish_odometry(const Odometry & odometry)
|
|
|
+{
|
|
|
+ static int frame_id = 0;
|
|
|
+ frame_id++;
|
|
|
+ Odometry msg = odometry;
|
|
|
+ msg.header.frame_id = origin_frame_id_;
|
|
|
+ msg.header.stamp = get_clock()->now();
|
|
|
+ msg.child_frame_id = simulated_frame_id_;
|
|
|
+ pub_odom_->publish(msg);
|
|
|
+
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::publish_tf(const Odometry & odometry)
|
|
|
+{
|
|
|
+ static int frame_id = 0;
|
|
|
+ frame_id++;
|
|
|
+ TransformStamped tf;
|
|
|
+ tf.header.stamp = get_clock()->now();
|
|
|
+ tf.header.frame_id = origin_frame_id_;
|
|
|
+ tf.child_frame_id = simulated_frame_id_;
|
|
|
+ tf.transform.translation.x = odometry.pose.pose.position.x;
|
|
|
+ tf.transform.translation.y = odometry.pose.pose.position.y;
|
|
|
+ tf.transform.translation.z = odometry.pose.pose.position.z;
|
|
|
+ tf.transform.rotation = odometry.pose.pose.orientation;
|
|
|
+
|
|
|
+ tf2_msgs::msg::TFMessage tf_msg{};
|
|
|
+ tf_msg.transforms.emplace_back(std::move(tf));
|
|
|
+ pub_tf_->publish(tf_msg);
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::publish_velocity(const VelocityReport & velocity)
|
|
|
+{
|
|
|
+ VelocityReport msg = velocity;
|
|
|
+ msg.header.stamp = get_clock()->now();
|
|
|
+ msg.header.frame_id = simulated_frame_id_;
|
|
|
+ pub_velocity_->publish(msg);
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::CalcXY(double flon,double flat,double & fx,double & fy)
|
|
|
+{
|
|
|
+ int zone{};
|
|
|
+ bool is_north{};
|
|
|
+ std::string mgrs_code;
|
|
|
+
|
|
|
+ double utm_x,utm_y;
|
|
|
+
|
|
|
+ try {
|
|
|
+ GeographicLib::UTMUPS::Forward(flat, flon, zone, is_north, utm_x, utm_y);
|
|
|
+ GeographicLib::MGRS::Forward(
|
|
|
+ zone, is_north, utm_x, utm_y, flat, 3, mgrs_code);
|
|
|
+ } catch (const GeographicLib::GeographicErr & err) {
|
|
|
+ std::cerr << err.what() << std::endl;
|
|
|
+ return;
|
|
|
+
|
|
|
+ }
|
|
|
+ fx = fmod(utm_x,1e5);
|
|
|
+ fy = fmod(utm_y,1e5);
|
|
|
+ return;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void pilot_autoware_bridge::publish_steering(const SteeringReport & steer)
|
|
|
+{
|
|
|
+
|
|
|
+ pub_steer_->publish(steer);
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::publish_acceleration()
|
|
|
+{
|
|
|
+ AccelWithCovarianceStamped msg;
|
|
|
+ msg.header.frame_id = "/base_link";
|
|
|
+ msg.header.stamp = get_clock()->now();
|
|
|
+ msg.accel.accel.linear.x = 0;//vehicle_model_ptr_->getAx();
|
|
|
+
|
|
|
+ using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
|
|
|
+ constexpr auto COV = 0.001;
|
|
|
+ msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x
|
|
|
+ msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y
|
|
|
+ msg.accel.covariance.at(COV_IDX::Z_Z) = COV; // linear z
|
|
|
+ msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV; // angular x
|
|
|
+ msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV; // angular y
|
|
|
+ msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV; // angular z
|
|
|
+ pub_acc_->publish(msg);
|
|
|
+}
|
|
|
+
|
|
|
+void pilot_autoware_bridge::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+{
|
|
|
+
|
|
|
+ (void)index;
|
|
|
+ (void)dt;
|
|
|
+ (void)strmemname;
|
|
|
+ iv::gps::gpsimu xgpsimu;
|
|
|
+ if(!xgpsimu.ParseFromArray(strdata,nSize))
|
|
|
+ {
|
|
|
+
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ double flat = xgpsimu.lat();
|
|
|
+ double flon = xgpsimu.lon();
|
|
|
+
|
|
|
+ double pitch,roll,yaw;
|
|
|
+ pitch = 0;
|
|
|
+ roll = 0;
|
|
|
+
|
|
|
+ yaw = 0;
|
|
|
+ Quaternion quat = ToQuaternion(yaw,pitch,roll);
|
|
|
+
|
|
|
+ nav_msgs::msg::Odometry msg;
|
|
|
+
|
|
|
+ double fx,fy;
|
|
|
+ CalcXY(flon,flat,fx,fy);
|
|
|
+
|
|
|
+ static double rel_x = fx;//7600;
|
|
|
+ static double rel_y = fy;//32100;
|
|
|
+ static double rel_z = 0;
|
|
|
+ static double vn = 0;
|
|
|
+ static double ve = 1;
|
|
|
+ // rel_x+=0.01;
|
|
|
+
|
|
|
+ msg.pose.pose.position.x = rel_x;
|
|
|
+ msg.pose.pose.position.y = rel_y;
|
|
|
+ msg.pose.pose.position.z = rel_z;
|
|
|
+
|
|
|
+ msg.pose.pose.orientation.x = quat.x;
|
|
|
+ msg.pose.pose.orientation.y = quat.y;
|
|
|
+ msg.pose.pose.orientation.z = quat.z;
|
|
|
+ msg.pose.pose.orientation.w = quat.w;
|
|
|
+
|
|
|
+ msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
|
|
|
+
|
|
|
+ publish_odometry(msg);
|
|
|
+ publish_tf(msg);
|
|
|
+
|
|
|
+ autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
|
|
|
+ velocity.longitudinal_velocity = 0;
|
|
|
+ velocity.lateral_velocity = 0.0F;
|
|
|
+ velocity.heading_rate = 0;
|
|
|
+ publish_velocity(velocity);
|
|
|
+
|
|
|
+ current_steer_.steering_tire_angle = static_cast<double>(0);
|
|
|
+ current_steer_.stamp = get_clock()->now();
|
|
|
+
|
|
|
+ publish_steering(current_steer_);
|
|
|
+
|
|
|
+ publish_acceleration();
|
|
|
+}
|
|
|
+
|
