在rviz中使用键盘控制burger

启动语句

roslaunch turtlebot3_fake turtlebot3_fake.launch //启动rviz、话题通信
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch //启动键盘控制节点

turtlebot3_fake.launch文件解读

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <param name="tb3_model" value="$(arg model)"/>

  <include file="$(find turtlebot3_bringup)/launch/includes/description.launch.xml">
    <arg name="model" value="$(arg model)" />
  include>  

  <node pkg="turtlebot3_fake" type="turtlebot3_fake_node" name="turtlebot3_fake_node" output="screen" />

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="50.0" />
  node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot3_fake)/rviz/turtlebot3_fake.rviz"/>
launch>

这个launch文件包含三部分：加载机器人模型、启动节点话题、启动rviz。

其中，turtlebot3_fake_node是自定义的一个节点。代码如下：

1. 主函数

int main(int argc, char* argv[])
{
     
  ros::init(argc, argv, "turtlebot3_fake_node");
  Turtlebot3Fake tb3fake;       
  ros::Rate loop_rate(30);

  while (ros::ok())
  {
     
    tb3fake.update();
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

定义turtlebot3_fake_node这个节点

2. Turtlebot3Fake::init()

发布burger的关节状态信息和里程计信息，订阅速度信息

// initialize publishers
  joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
  odom_pub_         = nh_.advertise<nav_msgs::Odometry>("odom", 100);

  // initialize subscribers
  cmd_vel_sub_  = nh_.subscribe("cmd_vel", 100,  &Turtlebot3Fake::commandVelocityCallback, this);

//回调函数
  void Turtlebot3Fake::commandVelocityCallback(const geometry_msgs::TwistConstPtr cmd_vel_msg)  
{
     
  last_cmd_vel_time_ = ros::Time::now();

  goal_linear_velocity_  = cmd_vel_msg->linear.x;
  goal_angular_velocity_ = cmd_vel_msg->angular.z;

  wheel_speed_cmd_[LEFT]  = goal_linear_velocity_ - (goal_angular_velocity_ * wheel_seperation_ / 2);  //根据全局的速度和角速度得到左右轮的速度
  wheel_speed_cmd_[RIGHT] = goal_linear_velocity_ + (goal_angular_velocity_ * wheel_seperation_ / 2);
}

订阅得到的速度、角速度定义为全局速度、全局角速度。通过全局速度以及全局角速度得到左右轮的速度。

3. tb3fake.update()

bool Turtlebot3Fake::update()   
{
     
  ros::Time time_now = ros::Time::now();
  ros::Duration step_time = time_now - prev_update_time_;
  prev_update_time_ = time_now;

  // zero-ing after timeout
  if((time_now - last_cmd_vel_time_).toSec() > cmd_vel_timeout_)
  {
     
    wheel_speed_cmd_[LEFT]  = 0.0;
    wheel_speed_cmd_[RIGHT] = 0.0;
  }

  // odom
  updateOdometry(step_time);
  odom_.header.stamp = time_now;
  odom_pub_.publish(odom_);   //发布更新过的里程计信息  odom_pub是发布者的名称，odom_是定义的数据类型名称

  // joint_states
  updateJoint();
  joint_states_.header.stamp = time_now;
  joint_states_pub_.publish(joint_states_);   //发布更新过的关节状态

  // tf
  geometry_msgs::TransformStamped odom_tf;
  updateTF(odom_tf);
  tf_broadcaster_.sendTransform(odom_tf);

  return true;
}

4. Turtlebot3Fake::updateOdometry(ros::Duration diff_time)

通过订阅得到的速度信息，更新里程计信息，包括位置、速度、角速度。通过update()函数将更新过的里程计信息发布出去。

bool Turtlebot3Fake::updateOdometry(ros::Duration diff_time)    //更新里程计信息，包括位置、速度、角速度
{
     
  double wheel_l, wheel_r; // rotation value of wheel [rad]
  double delta_s, delta_theta;
  double v[2], w[2];

  wheel_l = wheel_r     = 0.0;
  delta_s = delta_theta = 0.0;

  v[LEFT]  = wheel_speed_cmd_[LEFT];  //订阅得到的速度
  w[LEFT]  = v[LEFT] / WHEEL_RADIUS;  // w = v / r
  v[RIGHT] = wheel_speed_cmd_[RIGHT];
  w[RIGHT] = v[RIGHT] / WHEEL_RADIUS;

  last_velocity_[LEFT]  = w[LEFT];     //最新的角速度
  last_velocity_[RIGHT] = w[RIGHT];

  wheel_l = w[LEFT]  * diff_time.toSec();   //角度增量
  wheel_r = w[RIGHT] * diff_time.toSec();

  if(isnan(wheel_l))  //判断wheel_l是否为非数字值，若不是数字则条件为真
  {
     
    wheel_l = 0.0;
  }

  if(isnan(wheel_r))
  {
     
    wheel_r = 0.0;
  }

  last_position_[LEFT]  += wheel_l;   //一共转过的角度
  last_position_[RIGHT] += wheel_r;

  delta_s     = WHEEL_RADIUS * (wheel_r + wheel_l) / 2.0;   //位移增量
  delta_theta = WHEEL_RADIUS * (wheel_r - wheel_l) / wheel_seperation_;   //角度增量

  // compute odometric pose
  odom_pose_[0] += delta_s * cos(odom_pose_[2] + (delta_theta / 2.0));   //使用极坐标表示
  odom_pose_[1] += delta_s * sin(odom_pose_[2] + (delta_theta / 2.0));
  odom_pose_[2] += delta_theta;

  // compute odometric instantaneouse velocity    //计算瞬时速度
  odom_vel_[0] = delta_s / diff_time.toSec();     // v
  odom_vel_[1] = 0.0;
  odom_vel_[2] = delta_theta / diff_time.toSec(); // w

  odom_.pose.pose.position.x = odom_pose_[0];
  odom_.pose.pose.position.y = odom_pose_[1];
  odom_.pose.pose.position.z = 0;
  odom_.pose.pose.orientation = tf::createQuaternionMsgFromYaw(odom_pose_[2]);

  // We should update the twist of the odometry
  odom_.twist.twist.linear.x  = odom_vel_[0];
  odom_.twist.twist.angular.z = odom_vel_[2];

  return true;
}

启动rviz

首先打开一个空的rviz，Add需要的属性，save as 文件名为turtelbot3_fake.rviz。此时，会自动生成文件名为turtlebot3_fake.rviz的文件。

另一种turtlebot3_fake.launch写法

<launch>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="x_pos" default="0.0"/>
  <arg name="y_pos" default="0.0"/>
  <arg name="z_pos" default="0.0"/>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find turtlebot3_description)/urdf/turtlebot3_$(arg model).urdf.xacro" />  

  <node pkg="turtlebot3_fake" type="turtlebot3_fake_node" name="turtlebot3_fake_node" output="screen" />

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="50.0" />
  </node>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot3_fake)/rviz/turtlebot3_fake.rviz"/>
</launch>

这种写法与开头的.launch文件的不同之处在于加载burger的方式不同。

键盘控制代码见下篇。