I am using Ubuntu 20.04 and Gazebo 11.14 to simulate the TurtleBot 3 Waffle Pi robot. I run the code below, and record the command data and odmetry data form topic "/odom".
#include "ros/ros.h" #include <Eigen/Core> #include <Eigen/Dense> #include <iostream> #include "nav_msgs/Path.h" #include "nav_msgs/Odometry.h" #include <tf2_ros/buffer.h> #include <tf2/LinearMath/Matrix3x3.h> #include "tf2_ros/transform_listener.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "narrow_space_navigation/point_style.h" #include "narrow_space_navigation/utils.h" #include <random> Controller::car_state cur_pose; void odom_callback(const nav_msgs::Odometry::ConstPtr &msg) { try { geometry_msgs::Quaternion odom_quat = msg->pose.pose.orientation; tf2::Quaternion quat; tf2::fromMsg(odom_quat, quat); double roll, pitch, yaw; tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); cur_pose.x = msg->pose.pose.position.x; cur_pose.y = msg->pose.pose.position.y; cur_pose.yaw = yaw; cur_pose.v = msg->twist.twist.linear.x; cur_pose.w = msg->twist.twist.angular.z; } catch(const std::exception& e) { ROS_INFO("Some errors in the odom_callback."); } } int main(int argc, char *argv[]) { ros::init(argc,argv,"ramdom_test"); FILE* new_log; new_log = fopen("new_test.txt", "w"); ROS_INFO("ros init"); ros::NodeHandle nh; ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10); ros::Subscriber odom_sub = nh.subscribe("/odom", 10, odom_callback); int multiplier = atoi(argv[1]); geometry_msgs::Twist control_output; double dt = 0.02; double a = 0.0; ros::Rate r(50); while (ros::ok()) { a = a + dt; control_output.linear.x = 0.25 * (1 - cos(a)) * multiplier; double w_acc = 0.25 * sin(a) * multiplier; control_output.angular.z = control_output.angular.z + w_acc * dt; r.sleep(); ros::spinOnce(); twist_pub.publish(control_output); fprintf(new_log, "%.6f " "%.6f %.6f %.6f %.6f %.6f %.6f %.6f\n", cur_pose.x, cur_pose.y, cur_pose.yaw, control_output.linear.x, control_output.angular.z, cur_pose.v, cur_pose.w, w_acc); fflush(new_log); } return 0; } With the default gazebo urdf of the waffle pi robot, and multiplier = 4 ( which means the linear control speed cmd_v = 0.25 * multiplier * (1 - cos(a)), angular speed cmd_w = cmd_w + 0.25 * multiplier * sin(a), a += dt, please see the code above).
We have: (w_is is the angular speed from "/odom" and the w_cmd is the command, please neglect the w_should)
It is clear this urdf settings can not handle that much angular speed and angular acceleration. So I change the urdf into this:
<?xml version="1.0"?> <robot name="turtlebot3_waffle_pi_sim" xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:arg name="laser_visual" default="false"/> <xacro:arg name="camera_visual" default="false"/> <xacro:arg name="imu_visual" default="false"/> <gazebo reference="base_link"> <material>Gazebo/DarkGrey</material> </gazebo> <gazebo reference="wheel_left_link"> <mu1>1.0</mu1> <mu2>1.0</mu2> <soft_cfm>0.0</soft_cfm> <soft_erp>0.2</soft_erp> <kp>1e15</kp> <kd>4e12</kd> <minDepth>0.001</minDepth> <maxVel>10</maxVel> <fdir1>1 0 0</fdir1> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="wheel_right_link"> <mu1>1.0</mu1> <mu2>1.0</mu2> <soft_cfm>0.0</soft_cfm> <soft_erp>0.2</soft_erp> <kp>1e15</kp> <kd>4e12</kd> <minDepth>0.001</minDepth> <maxVel>10</maxVel> <fdir1>1 0 0</fdir1> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="caster_back_right_link"> <mu1>0.01</mu1> <mu2>0.01</mu2> <soft_cfm>0.0</soft_cfm> <soft_erp>0.2</soft_erp> <kp>1e15</kp> <kd>4e12</kd> <minDepth>0.001</minDepth> <maxVel>10</maxVel> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="caster_back_left_link"> <mu1>0.01</mu1> <mu2>0.01</mu2> <soft_cfm>0.0</soft_cfm> <soft_erp>0.2</soft_erp> <kp>1e15</kp> <kd>4e12</kd> <minDepth>0.001</minDepth> <maxVel>10</maxVel> <material>Gazebo/FlatBlack</material> </gazebo> <gazebo reference="imu_link"> <sensor type="imu" name="imu"> <always_on>true</always_on> <visualize>$(arg imu_visual)</visualize> </sensor> <material>Gazebo/Grey</material> </gazebo> <gazebo> <plugin name="turtlebot3_waffle_pi_controller" filename="libgazebo_ros_diff_drive.so"> <commandTopic>cmd_vel</commandTopic> <odometryTopic>odom</odometryTopic> <odometryFrame>odom</odometryFrame> <odometrySource>world</odometrySource> <publishOdomTF>true</publishOdomTF> <robotBaseFrame>base_footprint</robotBaseFrame> <publishWheelTF>false</publishWheelTF> <publishTf>true</publishTf> <publishWheelJointState>true</publishWheelJointState> <legacyMode>false</legacyMode> <updateRate>100</updateRate> <leftJoint>wheel_left_joint</leftJoint> <rightJoint>wheel_right_joint</rightJoint> <wheelSeparation>0.287</wheelSeparation> <wheelDiameter>0.066</wheelDiameter> <wheelAcceleration>4</wheelAcceleration> <wheelTorque>10</wheelTorque> <rosDebugLevel>na</rosDebugLevel> </plugin> </gazebo> <gazebo> <plugin name="imu_plugin" filename="libgazebo_ros_imu.so"> <alwaysOn>true</alwaysOn> <bodyName>imu_link</bodyName> <frameName>imu_link</frameName> <topicName>imu</topicName> <serviceName>imu_service</serviceName> <gaussianNoise>0.0</gaussianNoise> <updateRate>0</updateRate> <imu> <noise> <type>gaussian</type> <rate> <mean>0.0</mean> <stddev>2e-4</stddev> <bias_mean>0.0000075</bias_mean> <bias_stddev>0.0000008</bias_stddev> </rate> <accel> <mean>0.0</mean> <stddev>1.7e-2</stddev> <bias_mean>0.1</bias_mean> <bias_stddev>0.001</bias_stddev> </accel> </noise> </imu> </plugin> </gazebo> <gazebo reference="base_scan"> <material>Gazebo/FlatBlack</material> <sensor type="ray" name="lds_lfcd_sensor"> <pose>0 0 0 0 0 0</pose> <visualize>$(arg laser_visual)</visualize> <update_rate>5</update_rate> <ray> <scan> <horizontal> <samples>360</samples> <resolution>1</resolution> <min_angle>0.0</min_angle> <max_angle>6.28319</max_angle> </horizontal> </scan> <range> <min>0.120</min> <max>3.5</max> <resolution>0.015</resolution> </range> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.01</stddev> </noise> </ray> <plugin name="gazebo_ros_lds_lfcd_controller" filename="libgazebo_ros_laser.so"> <topicName>scan</topicName> <frameName>base_scan</frameName> </plugin> </sensor> </gazebo> <!--link : https://www.raspberrypi.org/documentation/hardware/camera/--> <gazebo reference="camera_rgb_frame"> <sensor type="camera" name="Pi Camera"> <always_on>true</always_on> <visualize>$(arg camera_visual)</visualize> <camera> <horizontal_fov>1.085595</horizontal_fov> <image> <width>640</width> <height>480</height> <format>R8G8B8</format> </image> <clip> <near>0.03</near> <far>100</far> </clip> </camera> <plugin name="camera_controller" filename="libgazebo_ros_camera.so"> <alwaysOn>true</alwaysOn> <updateRate>30.0</updateRate> <cameraName>camera</cameraName> <frameName>camera_rgb_optical_frame</frameName> <imageTopicName>rgb/image_raw</imageTopicName> <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName> <hackBaseline>0.07</hackBaseline> <distortionK1>0.0</distortionK1> <distortionK2>0.0</distortionK2> <distortionK3>0.0</distortionK3> <distortionT1>0.0</distortionT1> <distortionT2>0.0</distortionT2> </plugin> </sensor> </gazebo> </robot> Now the control graph is:
It looks good but there is a significant fluctuation at the begining, and if I run another code, which is a model predict controller to follow a path. The graph is:
we still get some significant fluctuations, in this picture, at the end of the control, which will make the final precision not that good.
I wonder where this fluctuation comes from?
I observe that the robot does perform this fluctuating angular velocity, so maybe it's not just the odom, right? And these fluctuations seem to be in the direction to the X-axis, are they interspersed with some unknown 0 angular velocity control signals outside my control? Is this potentially related to my control algorithm? (The command is smooth enough, so I doubt it's the cause.)
Could it be due to the modifications made in the Gazebo URDF file or specific settings in Gazebo?
I'm uncertain, so I would appreciate any insights you might have if you're familiar with the Gazebo simulation platform. Thank you for your help!
Edit 1: I change the simulation step to 0.1 ms and update rate to 10000, but the fluctuation problem didn't go away, it seems got worse (just did a couple of tests).
Edit 2: if I reduce the update rate of the diff control in the gazebo urdf file, the fluctuation get worser. (Though with 1000HZ the big fluctuations are still there)
Edit 3: In the mpc code, I track a path of bezier curve shape, and I get a S-curve speed profile with it. The default linear acc_max of the S-curve is 0.5m/s^2. Now I change it to 0.1m/s^2 (0.3 still has problem), the angular graph now looks like:
It looks good! Except for the ending when I force a rapid stop. So it seems like the model can handle that much acceleration?
