The C++ implementation of twist_to_motor.py is discussed in this section.
This node will convert the twist message (geometry_msgs/Twist) to motor target velocities. The topics subscribing by this node is the twist message from teleop node or Navigation stack and it publishes the target velocities for the two motors. The target velocities are fed into the PID nodes, which will send appropriate commands to each motor. The CPP file name is twist_to_motor.cpp and you can get it from the chapter_9_codes/chefbot_navig_cpp/src folder.

TwistToMotors::TwistToMotors() 
{ 
  init_variables(); 
  get_parameters(); 
   
  ROS_INFO("Started Twist to Motor node"); 
   
  cmd_vel_sub = n.subscribe("cmd_vel_mux/input/teleop",10, &TwistToMotors::twistCallback, this); 
   
  pub_lmotor = n.advertise<std_msgs::Float32>("lwheel_vtarget", 50); 
 
  pub_rmotor = n.advertise<std_msgs::Float32>("rwheel_vtarget", 50); 
   
}

The following code snippet is the callback function of the twist message. The linear velocity X is assigned as dx, Y as dy, and angular velocity Z as dr.

void TwistToMotors::twistCallback(const geometry_msgs::Twist &msg) 
{ 
 
  ticks_since_target = 0; 
   
  dx = msg.linear.x; 
  dy = msg.linear.y; 
  dr = msg.angular.z; 
 
} 

After getting dx, dy, and dr, we can compute the motor velocities using the
following equations:

dx = (l + r) / 2

dr = (r - l) / w

Here r and l are the right and left wheel velocities, and w is the base width. The preceding equations are implemented in the following code snippet. After computing the wheel velocities, it is published to the lwheel_vtarget and rwheel_vtarget topics.

  right = ( 1.0 * dx ) + (dr * w /2); 
  left = ( 1.0 * dx ) - (dr * w /2); 
 
 
  std_msgs::Float32 left_; 
  std_msgs::Float32 right_; 
 
  left_.data = left; 
  right_.data = right; 
 
 
  pub_lmotor.publish(left_); 
  pub_rmotor.publish(right_); 
 
  ticks_since_target += 1; 
 
  ros::spinOnce();