cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

目前采用只使用陀螺仪z轴数据

Browse Source
This commit is contained in:
Orange
2000-01-01 08:58:21 +08:00
parent 2f51de27e5
commit 48452d8d93
4 changed files with 593 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/origincar_base/CMakeLists.txt
View File

@@ -62,16 +62,18 @@ include_directories(
)
set(origincar_base_node_SRCS
src/origincar_base.cpp
src/Quaternion_Solution.cpp
)
add_executable(origincar_base_node src/origincar_base.cpp src/Quaternion_Solution.cpp)
add_executable(origincar_base_node src/origincar_base.cpp)
ament_target_dependencies(origincar_base_node tf2_ros tf2 tf2_geometry_msgs rclcpp std_msgs geometry_msgs robot_localization nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg origincar_description)
#add_executable(testNode src/test.cpp src/Quaternion_Solution.cpp)
#ament_target_dependencies(testNode rclcpp std_msgs nav_msgs std_srvs sensor_msgs ackermann_msgs serial origincar_msg)
install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME})
install(PROGRAMS
scripts/cmd_vel_to_ackermann_drive.py
DESTINATION lib/${PROJECT_NAME}
)
install(TARGETS
origincar_base_node

2
src/origincar_base/include/origincar_base/origincar_base.h
View File

@@ -143,6 +143,7 @@ private:
void Publish_ImuSensor();
void Publish_Voltage();
void Publish_GyroDebug();
auto createQuaternionMsgFromYaw(double yaw);
bool Get_Sensor_Data();
@@ -192,6 +193,7 @@ private:
rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotpose_publisher;
rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr robotvel_publisher;
rclcpp::Publisher<origincar_msg::msg::Data>::SharedPtr gyrodebug_publisher;
// rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr tf_pub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_bro;

17
src/origincar_base/src/origincar_base.cpp
View File

@@ -1,6 +1,5 @@
#include "origincar_base/origincar_base.h"
#include "rclcpp/rclcpp.hpp"
#include "origincar_base/Quaternion_Solution.h"
#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
#include "origincar_msg/msg/data.hpp"
#include "robot_localization/srv/set_pose.hpp"
@@ -144,10 +143,6 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr
{
memset(&Robot_Pos, 0, sizeof(Robot_Pos));
memset(&Mpu6050, 0, sizeof(Mpu6050));
q0 = 0;
q1 = 0;
q2 = 0;
q3 = 0;
Robot_Pos.X = 0.54;
Robot_Pos.Y = 0.2;
Robot_Pos.Z = 0;
@@ -180,14 +175,13 @@ void origincar_base::Sign_Switch_Callback(const std_msgs::msg::Int32::SharedPtr
void origincar_base::Publish_ImuSensor()
{
tf2::Quaternion q;
q.setRPY(0.0, 0.0, Robot_Pos.Z);
sensor_msgs::msg::Imu Imu_Data_Pub;
Imu_Data_Pub.header.stamp = rclcpp::Node::now();
Imu_Data_Pub.header.frame_id = gyro_frame_id;
Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
Imu_Data_Pub.orientation = tf2::toMsg(q);
Imu_Data_Pub.orientation_covariance[0] = 1e6;
Imu_Data_Pub.orientation_covariance[4] = 1e6;
Imu_Data_Pub.orientation_covariance[8] = 1e-6;
@@ -448,9 +442,6 @@ void origincar_base::Control()
Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125
Robot_Pos.Z += Robot_Vel.Z * Sampling_Time;
Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Robot_Vel.Z,
Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
Publish_ImuSensor();
Publish_GyroDebug();
Publish_Voltage();