实验步骤：

spark机器人控制（2）--编写节点完成对机器人的控制

任务1：编写节点实现对机器人前进一定距离并返回原地的控制

任务2：编写节点控制机器人行走一个正方形

任务1实现方法：

预备工作：启动spark机器人、通过VNCViewer连接到spark机器人

步骤1、创建工作区

注：如果该步骤之前已完成，则可以省略

（注：其中ros_tut为工作区的名字，请更改为你自己的名字（可以用自己的名字拼音简写）

$ mkdir  -p  ~/ros_tut/src
$ cd  ~/ros_tut/src
$ catkin_init_workspace
$ cd  ~/ros_tut
$ catkin_make
$ source  ~/ros_tut/devel/setup.bash
$ rospack  profile
$ echo  "source ~/ros_tut/devel/setup.bash" >> ~/.bashrc

步骤2、创建包（如果该步骤之前已经完成，则可省略）

（注：其中simple_nodes为包的名字,可以根据自己的喜好更改，建议A班用simple_nodesa,B班用simple_nodesb）

$ cd  ~/ros_tut/src
$catkin_create_pkg  simple_nodes  std_msgs  geometry_msgs  turtlesim rospy roscpp
$ cd  ~/ros_tut
$ catkin_make

步骤3、构建机器人控制节点

编写diff2w_ctrl01.cpp文件

/*
 * 节点diff2w_ctrl01 
   控制两轮差速转向机器人，前进给定的距离，然后返回原位 
 * 向/cmd_vel话题发布指令序列
 * 开发者：段琢华,duanzhuohua@163.com 2022-4-20
*/

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
const double PI = 3.1415926535898;

ros::Rate *loop_rate;
ros::Publisher vel_pub;

int main(int argc, char **argv)
{
  ros::init(argc, argv, "diff2w_ctrl");
  if (argc < 2)
  {//命令行中处理命令本身，还要一个额外的参数distance
   //如果参数不够，提示使用方法并退出
    ROS_INFO("usage: %s distance(m)",argv[0]);
    return 1;
  }
  double distance;
  distance = atof(argv[1]);//获取请求消息的第一个参数，atof函数把字符串转换为浮点数

  ros::NodeHandle n;
  ROS_INFO("前进%f米，然后返回，开始了...",distance); 

  vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

  loop_rate = new ros::Rate(1); 
  


  double linear_speed=0.2,angular_speed=PI/6; 
  double dist_togo=distance;//还需要走的距离
  int go_or_back=1;//1:前进、2、旋转180度，3、前进、4、旋转180度


   
  geometry_msgs::Twist cmd_vel;

  //步骤1：前进
  while(ros::ok() && dist_togo>linear_speed) 
  {
    cmd_vel.linear.x = linear_speed;
    vel_pub.publish(cmd_vel);
    dist_togo-=linear_speed;
    loop_rate->sleep();
  } 
  cmd_vel.linear.x = dist_togo;
  vel_pub.publish(cmd_vel);
  loop_rate->sleep();

  //步骤2：旋转180度
  int count=0;
  while(ros::ok() && count<6) 
  {
    cmd_vel.linear.x=0;
    cmd_vel.angular.z = angular_speed;
    vel_pub.publish(cmd_vel);
    count++;
    loop_rate->sleep();
  } 

  //步骤3：前进
  dist_togo=distance;
  while(ros::ok() && dist_togo>linear_speed) 
  {
    cmd_vel.linear.x = linear_speed;
    cmd_vel.angular.z=0;
    vel_pub.publish(cmd_vel);
    dist_togo-=linear_speed;
    loop_rate->sleep();
  } 
  cmd_vel.linear.x = dist_togo;
  vel_pub.publish(cmd_vel);
  loop_rate->sleep();
  
  //步骤4：旋转180度
  count=0;
  while(ros::ok() && count<6) 
  {
    cmd_vel.linear.x=0;
    cmd_vel.angular.z = angular_speed;
    vel_pub.publish(cmd_vel);
    count++;
    loop_rate->sleep();
  }
  
  //停止 
  if (ros::ok()){
    cmd_vel.linear.x=0;
    cmd_vel.angular.z=0;
    vel_pub.publish(cmd_vel);//stop
   }


  return 0;
}

新建一个终端

$roscd  simple_nodes

注：执行上述指令后，当前目录应该为~/ros_tut/src/simple_nodes$，在此基础上继续执行以下指令

$cd  src

将diff2w_ctrl01.cpp下载（拷贝至该当前目录，拷贝的方法，通过scp命令：台式机与spark联通后，cmd启动台式机终端，转到diff2w_ctrl01.cpp文件所在的目录，通过下面的指令进行复制：

scp diff2w_ctrl01.cpp spark@10.42.0.1:~/ros_tut/src/simple_nodes/src  

 按提示输入yes,密码输入spark）

拷贝成功后,在spark机器人端通过ls命令可以看到diff2w_ctrl01.cpp

$cd  ..

$gedit   CMakeLists.txt

将下面的两行加入到文件末尾，然后保存文件并退出编辑

add_executable(diff2w_ctrl01 src/diff2w_ctrl01.cpp)
target_link_libraries(diff2w_ctrl01 ${catkin_LIBRARIES})

$cd  ~/ros_tut

$catkin_make

构建成功后继续后面的步骤，否则根据提示信息修改源程序，再通过步骤3来构建。

步骤4：测试节点

新开一个终端，启动机器人底盘控制节点

$roslaunch  spark_teleop  teleop.launch

确保机器人前方有足够的自由空间，然后另起一个终端，执行下面的指令

$rosrun  simple_nodes  diff2w_ctrl01 1

(参数1为前进的距离，单位为m),观测机器人运动情况。

任务2：编写节点控制机器人行走一个正方形

参照diff2w_ctrl01.cpp，编写diff2w_ctrl02.cpp,可以按下面的方法复制，然后编辑

$roscd  simple_nodes

$cd  src

$cp  diff2w_ctrl01.cpp diff2w_ctrl02.cpp

设计正方形控制的代码，编辑后参考任务1的步骤3的方式来构建节点，参考任务1的步骤4来测试节点。

扩展：

（1）编写节点，实现移动机器人的加速度和减速度控制（梯形速度控制）

设置参数，最大速度，加速时间（到最大速度的时间），最大速度运行时间，减速（到停止）时间

（2）进一步扩展，结合感知：在条件允许时（机器人前进方向的前方空阔），在前方有障碍物

时紧急制动(减速)（结合激光雷达感知）。

注意力机制的应用：重点注意机器人轨迹方向的区域，例如，机器指令为左转，则重点看

左边区域（其实就是DWA的简化）

关于Spark的最大速度的确认，源码分析：

在spark_base.cpp中

sparkbase->drive(cmd_vel->linear.x, cmd_vel->angular.z);

在spark_base_interface.cpp中

int nxsparkbase::OpenInterface::drive(double linear_speed, double angular_speed)
{
  // int left_speed_mm_s =
  // (int)((linear_speed-SPARKBASE_AXLE_LENGTH*angular_speed/2)*1e3);		// Left
  // wheel velocity in mm/s
  // int right_speed_mm_s =
  // (int)((linear_speed+SPARKBASE_AXLE_LENGTH*angular_speed/2)*1e3);	// Right
  // wheel velocity in mm/s
  //\u8c03\u6362\u4e86\u5de6\u53f3\u8f6e\u5b50\u7684\u901f\u5ea6,\u4f7f\u5f97\u534a\u5f27\u5411\u540e
  int left_speed_mm_s =
      (int)((linear_speed - SPARKBASE_AXLE_LENGTH * angular_speed / 2) * 1e3);  // Left wheel velocity in mm/s
  int right_speed_mm_s =
      (int)((linear_speed + SPARKBASE_AXLE_LENGTH * angular_speed / 2) * 1e3);  // Right wheel velocity in mm/s

  //    printf("%d,%d\n",left_speed_mm_s,right_speed_mm_s);
  return this->driveDirect(left_speed_mm_s, right_speed_mm_s);
}
// Set the motor speeds
int nxsparkbase::OpenInterface::driveDirect(int left_speed, int right_speed)
{
  // Limit velocity
  int16_t left_speed_mm_s = MAX(left_speed, -SPARKBASE_MAX_LIN_VEL_MM_S);
  left_speed_mm_s = MIN(left_speed, SPARKBASE_MAX_LIN_VEL_MM_S);
  int16_t right_speed_mm_s = MAX(right_speed, -SPARKBASE_MAX_LIN_VEL_MM_S);
  right_speed_mm_s = MIN(right_speed, SPARKBASE_MAX_LIN_VEL_MM_S);

  int16_t left_speed_pwm_s = left_speed_mm_s / SPARKBASE_PULSES_TO_MM;
  int16_t right_speed_pwm_s = right_speed_mm_s / SPARKBASE_PULSES_TO_MM;

  // printf("left_wheel_speed,right_wheel_speed,%d,%d\n",left_speed_pwm_s,right_speed_pwm_s);
  return this->drivePWM(left_speed_pwm_s, right_speed_pwm_s);
  return (0);
}

另外，在spark_base_interface.h中

// Sparkbase Dimensions
#define SPARKBASE_BUMPER_X_OFFSET 0.050
#define SPARKBASE_DIAMETER 0.340
#define SPARKBASE_AXLE_LENGTH 0.266

#define SPARKBASE_MAX_LIN_VEL_MM_S 500
#define SPARKBASE_MAX_ANG_VEL_RAD_S 2
#define SPARKBASE_MAX_RADIUS_MM 2000

思考：请根据两轮差速转向系统运动学模型，计算其最大的线速度，及最大的角速度。