pose prediction fix

neobotix · Jul 16, 2020 · 8a18423 · 8a18423
1 parent e67d0ad
commit 8a18423
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/NeoLocalPlanner.cpp b/src/NeoLocalPlanner.cpp
@@ -224,7 +224,7 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
 	{
 		const double midpoint_yaw = start_yaw + start_yawrate * m_lookahead_time / 2;
 		actual_pos = local_pose.getOrigin() + tf2::Matrix3x3(createQuaternionFromYaw(midpoint_yaw))
-												* tf2::Vector3(start_vel_x * m_lookahead_time, 0, 0);
+												* tf2::Vector3(start_vel_x, start_vel_y, 0) * m_lookahead_time;
 		actual_yaw = start_yaw + start_yawrate * m_lookahead_time;
 	}
 	const tf2::Transform actual_pose = tf2::Transform(createQuaternionFromYaw(actual_yaw), actual_pos);