Robotics StackExchange | Archived questions

laser pattern turning along with robot in Rviz

I am having problems with my laser pattern in Rviz.

In my odometry, I checked that:

My problem: When I spin the robot around its own Z, the grid and the map stand still in Rviz, but the rplidar laser pattern turns along with the robot. (the red laser pattern behaves as if the entire room turns together with the robot). This problem does not occur when I move the robot in a straight line. Only when I turn.

Result is that when I set out a goal, the robot drives around orientlessly like drunk as the laser pattern is not consistent with the fixed world.

Relevant patameters in my RVIZ:

relevant link in my urdf:

 <joint name="base_link_to_laser" type="fixed"> <!-- laser pos relative to origin vehicle -->
   <parent link="base_link"/>
   <child link="laser"/>
   <origin xyz="0.33 0 0.15" rpy = "0 0 3.1415" />
 </joint>

relevant part of my C++ code, using the counter status of both motor encoders:

    //***** update transforms ****             

    ros::Time t = ros::Time::now();
    double dt = (t - tPrev).toSec();

    double dWheelSpinAngle1 = (cnt1 - cnt1Prev) * 2 * M_PI / (EncoderFactor * GearRate);//rad
    double dWheelSpinAngle2 = (cnt2 - cnt2Prev) * 2 * M_PI / (EncoderFactor * GearRate);//rad

    double wheelDisplacement1 = dWheelSpinAngle1 * 0.5 * WheelDiam * DiamtrCorrec1; //m
    double wheelDisplacement2 = dWheelSpinAngle2 * 0.5 * WheelDiam * DiamtrCorrec2; //m
    double dVehicleLin_x = (wheelDisplacement1 + wheelDisplacement2) / 2; //m
    double dVehicleLin_y = 0;// we can't drive sideways
    double dVehicleAng =  (wheelDisplacement1 - wheelDisplacement2) / WheelDistance; //rad

    double vx, vy, vth;
    if (dt > 0)
    {
       vx = dVehicleLin_x / dt;
       vy = 0;// we can't drive sideways
       vth = dVehicleAng / dt;
    }
    else
    {
       vx = 0;
       vy = 0;
       vth = 0;
    }

    //update position transform:
    double x = xPrev + (dVehicleLin_x * cos(thPrev) - dVehicleLin_y * sin(thPrev));//new x pos. 
    double y = yPrev + (dVehicleLin_x * sin(thPrev) + dVehicleLin_y * cos(thPrev));//new y pos. 
    double z = 0;// we can't fly
    double th = thPrev + dVehicleAng; //vehicle's new orientation 

    Transformed Tra;            

    //Trans values:

    //header:
    Tra.odomTrans.header.stamp = t; //current time
    Tra.odomTrans.header.frame_id = "odom"; //world frame
    Tra.odomTrans.child_frame_id = "base_link"; //vehicle frame 

    printf("\ncalculating transforms: \n");

    //displacement:
    Tra.odomTrans.transform.translation.x = x; printf("x: %g ", x);
    Tra.odomTrans.transform.translation.y = y; printf("  y: %g \n\n", y);
    Tra.odomTrans.transform.translation.z = 0.0; 

    //rotation:             
    Tra.odomTrans.transform.rotation = tf::createQuaternionMsgFromYaw(th);   


    //Pose values:

    printf("\ncalculating poses: \n");
    //header:
    Tra.odomPose.header.frame_id = "odom";//topic
    Tra.odomPose.header.stamp = t;
    Tra.odomPose.child_frame_id = "base_link";//vehicle's coordinate frame

    //position:
    Tra.odomPose.pose.pose.position.x = x;
    Tra.odomPose.pose.pose.position.y = y;
    Tra.odomPose.pose.pose.position.z = 0.0;

    //orientation:
    Tra.odomPose.pose.pose.orientation.w = Tra.odomTrans.transform.rotation.w;
    Tra.odomPose.pose.pose.orientation.x = Tra.odomTrans.transform.rotation.x;
    Tra.odomPose.pose.pose.orientation.y = Tra.odomTrans.transform.rotation.y;
    Tra.odomPose.pose.pose.orientation.z = Tra.odomTrans.transform.rotation.z;

    //speed:
    Tra.odomPose.twist.twist.linear.x = vx; 
    Tra.odomPose.twist.twist.linear.y = vy; 
    Tra.odomPose.twist.twist.linear.z = 0.0;
    Tra.odomPose.twist.twist.angular.z = vth; 

    // *** send updated transforms to Ros ***

    //broadcast updated transform:
    odom_broadcaster.sendTransform(Tra.odomTrans);//send updated vehicle transforms to Ros 

    //publish updated odometry:
    odom_pub.publish(Tra.odomPose);//send updated odometry to Ros

    //update variables for next iteration in while loop:
    xPrev = x;
    yPrev = y;
    thPrev = th;
    std::cout << "\ndelta t:" << ros::Time::now() - tPrev << "\n";
    tPrev = ros::Time::now();
    cnt1Prev = cnt1;
    cnt2Prev = cnt2;

Part of the logging:

update frame 0
update ld=0 ad=0
Laser Pose= 0 0 0.0373624
m_count 0
Registering First Scan
[ INFO] [1647679041.043851111]: Resizing costmap to 4000 X 4000 at 0.100000 m/pix
[ INFO] [1647679041.401679443]: Received a 4000 X 4000 map at 0.100000 m/pix
[ INFO] [1647679041.405547348]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1647679041.420123005]:     Subscribed to Topics: laser_scan_sensor point_cloud_sensor
[ INFO] [1647679041.457058984]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1647679041.543872230]: local_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1647679041.560145421]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1647679041.564847654]:     Subscribed to Topics: laser_scan_sensor point_cloud_sensor
[ INFO] [1647679041.600456693]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1647679041.687847999]: Created local_planner dwa_local_planner/DWAPlannerROS
[ INFO] [1647679041.692913368]: Sim period is set to 0.20
[ INFO] [1647679042.169009634]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1647679042.177466480]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1647679042.243121758]: Resizing costmap to 4000 X 4000 at 0.050000 m/pix
[ INFO] [1647679042.587871595]: odom received!

update frame 491
update ld=0 ad=0.523073
Laser Pose= 0 0 0.0373624
m_count 1
Average Scan Matching Score=1804.75
neff= 29.7715
Registering Scans:Done

update frame 945    
update ld=0 ad=0.523073
Laser Pose= 0 0 0.112087
m_count 2
Average Scan Matching Score=1863.22
neff= 29.7374
Registering Scans:Done

Asked by RH56 on 2022-03-17 06:30:25 UTC

Comments

rviz is transforming each data point in the scan message from the laser frame to the map frame. So it is likely that one of the TF are not being updated correctly as base_link moves.

Asked by Mike Scheutzow on 2022-03-18 07:10:44 UTC

Thanks, Mike. I double-checked my C++ code but can't find anything wrong. Is there anything missing in my transforms that I overlooked?

Asked by RH56 on 2022-03-19 02:49:11 UTC

Answers