ROS_Robot/wpr_simulation2
1
0
Fork 0
mirror of https://github.com/6-robot/wpr_simulation2.git synced 2025-09-15 12:58:54 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update imu_behavior

Browse Source
This commit is contained in:
Robot 2023-10-10 08:06:07 +08:00
parent fcef2c66e5
commit 9e417c7119
1 changed files with 2 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
demo_cpp/6_imu_behavior.cpp
View File

@ -24,14 +24,12 @@ void IMUCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
yaw = yaw * 180 / M_PI;
RCLCPP_INFO(node->get_logger(), "roll= %.0f pitch= %.0f yaw= %.0f", roll, pitch, yaw);
geometry_msgs::msg::Twist vel_msg;
double target_yaw = 90;
geometry_msgs::msg::Twist vel_msg;
double diff_angle = target_yaw - yaw;
vel_msg.angular.z = diff_angle * 0.01;
vel_msg.linear.x = 0.1;
vel_pub->publish(vel_msg);
}
@ -39,11 +37,10 @@ int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
node = rclcpp::Node::make_shared("demo_imu_behavior");
node = rclcpp::Node::make_shared("imu_behavior_node");
auto sub = node->create_subscription<sensor_msgs::msg::Imu>("imu", 100, IMUCallback);
// 发布速度控制话题
vel_pub = node->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);
rclcpp::spin(node);