laser pattern turning along with robot in Rviz
I am having problems with my laser pattern in Rviz.
In my odometry, I checked that:
- When driving 4 meter in a straight line, the number of encoder ticks in both wheels correspond with the calculated number over that distance.
- When driving 1 meter forward, in Rviz the robot also travels approximately 1 grid square
- When turning the robot 90 degrees anticlockwise, in Rviz it also turns approximately 90 degrees anticlockwise
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:
- Global Options, Fixed Frame: map
- Grid, Reference Frame: Fixed Frame
- Map, Topic: /map
- Local Planner, Topic: /movebase/DWAPlannerROS/localplan
- LocalCostmap, Topic: /movebase/localcostmap/costmap
- Global Planner, Topic: /movebase/DWAPlannerROS/globalplan
- GlobalCostmap, Topic: /movebase/globalcostmap/costmap
- Robot Footprint, Topic: /movebase/localcostmap/footprint
- Path, Topic: movebase/DWAPlannerROS/localplan
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 themap
frame. So it is likely that one of the TF are not being updated correctly asbase_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