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?