0
$\begingroup$

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)

enter image description here

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:

enter image description here

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:

enter image description here

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).

smaller step with edited urdf model

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:

enter image description here

It looks good! Except for the ending when I force a rapid stop. So it seems like the model can handle that much acceleration?

$\endgroup$

1 Answer 1

1
$\begingroup$

gazebo is a physics simulator. By default, the robot moves because of friction of the rotating wheels against the ground. So just because you send a new velocity command does not mean the robot-object is going to exactly do that in the simulation. The robot's inertia is taken into account, as is possible wheel-slippage against the ground.

Another factor is that the step size of the simulation can affect the output. You want to reduce this to something like 1 millisecond (or even 0.1 millisecond if your CPU can handle it.)

$\endgroup$
2
  • $\begingroup$ Thank you for your reply, I will try to reduce the simulation steps and see if this big fluctuation will disappear. If it still exists, then it may be a problem with the model. $\endgroup$ Commented Oct 12 at 6:35
  • $\begingroup$ I change the step to a smaller one (0.1 ms), but the significant fluctuation still there, maybe even worser. I edit the qustion with a new graph to show that. So maybe it is a problem with model? Or the world file, some gazebo settings? $\endgroup$ Commented Oct 15 at 5:33

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.