从turtlesim到贪吃蛇……（补充）

原文如下：

从turtlesim到贪吃蛇……

部分提示：

背下来但是不理解，忘得特别快！！！

综合项目案例一：贪吃蛇实现
1. 这个项目实现，需要用到哪些已经学过的知识点？
总-分
2. 如何将这些知识点融合（综合）完成此案例？
3. 每个知识点在此综合项目中的组成部分或位置？
4. 设计方案（草案）完成项目构思？
5. 编程实现各部分的功能并测试？
6. 将各部分的功能进行有机组合实现此项目案例？
分-总
综合拆解成单元，单元组成综合。
复杂变简单，简单变复杂

1. 车队现象！
领队负责路径规划，其他参与者负责跟随
第一种：领队找队友-贪吃蛇
第二种：队友找领队-集群

2. 图形化使用turtlesim；扩展生成新的机器人（用cpp代码实现而非API接口）；领队需要使用键盘遥控；

3. 集群和贪吃蛇联系，如何改变和应用？

4. 如何利用上述知识点进行设计？

一个有问题的程序如下（请不要参考）： 

  

   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      #include <ros/ros.h>
     
    
   

    

     

    
    

     

      #include <tf/transform_listener.h>
     
    
   

    

     

    
    

     

      #include <geometry_msgs/Twist.h>
     
    
   

    

     

    
    

     

      #include <turtlesim/Spawn.h>
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      int main(int argc, char** argv)
     
    
   

    

     

    
    

     

      {
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::init(argc, argv, "my_tf_listener");
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::NodeHandle node;
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle3 = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv3;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv3);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle4 = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv4;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv4);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle5 = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv5;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv5);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle6 = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv6;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv6);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::service::waitForService("/spawn");
     
    
   

    

     

    
    

     

      	ros::ServiceClient add_turtle7 = node.serviceClient<turtlesim::Spawn>("/spawn");
     
    
   

    

     

    
    

     

      	turtlesim::Spawn srv7;
     
    
   

    

     

    
    

     

      	add_turtle.call(srv7);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel3 = node.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel4 = node.advertise<geometry_msgs::Twist>("/turtle4/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel5 = node.advertise<geometry_msgs::Twist>("/turtle5/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel6 = node.advertise<geometry_msgs::Twist>("/turtle6/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	ros::Publisher turtle_vel7 = node.advertise<geometry_msgs::Twist>("/turtle7/cmd_vel", 10);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	tf::TransformListener listener;
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      	tf::TransformListener listener3;
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
	ros::Rate rate(10.0);
     
    
   

    

     

    
    

     
	while (node.ok())
     
    
   

    

     

    
    

     

      	{
     
    
   

    

     

    
    

     
		// 
     
    
   

    

     

    
    

     

      		tf::StampedTransform transform;
     
    
   

    

     

    
    

     
		try
     
    
   

    

     

    
    

     

      		{
     
    
   

    

     

    
    

     

      			listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
     
    
   

    

     

    
    

     

      			listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
     
    
   

    

     

    
    

     

      		}
     
    
   

    

     

    
    

     
		catch (tf::TransformException &ex) 
     
    
   

    

     

    
    

     

      		{
     
    
   

    

     

    
    

     
			ROS_ERROR("%s",ex.what());
     
    
   

    

     

    
    

     

      			ros::Duration(1.0).sleep();
     
    
   

    

     

    
    

     
			continue;
     
    
   

    

     

    
    

     

      		}
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
		// 
     
    
   

    

     

    
    

     

      		geometry_msgs::Twist vel_msg;
     
    
   

    

     

    
    

     

      		vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
     
    
   

    

     

    
    

     

      				                        transform.getOrigin().x());
     
    
   

    

     

    
    

     

      		vel_msg.linear.x = 2.0 * sqrt(pow(transform.getOrigin().x(), 2) +
     
    
   

    

     

    
    

     
				                      pow(transform.getOrigin().y(), 2));
     
    
   

    

     

    
    

     

      		turtle_vel.publish(vel_msg);
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
		if(vel_msg.linear.x<0.01){
     
    
   

    

     

    
    

     
			// 
     
    
   

    

     

    
    

     

      			tf::StampedTransform transform3;
     
    
   

    

     

    
    

     
			try
     
    
   

    

     

    
    

     

      			{
     
    
   

    

     

    
    

     

      				listener3.waitForTransform("/turtle3", "/turtle2", ros::Time(0), ros::Duration(3.0));
     
    
   

    

     

    
    

     

      				listener3.lookupTransform("/turtle3", "/turtle2", ros::Time(0), transform);
     
    
   

    

     

    
    

     

      			}
     
    
   

    

     

    
    

     
			catch (tf::TransformException &ex) 
     
    
   

    

     

    
    

     

      			{
     
    
   

    

     

    
    

     
				ROS_ERROR("%s",ex.what());
     
    
   

    

     

    
    

     

      				ros::Duration(1.0).sleep();
     
    
   

    

     

    
    

     
				continue;
     
    
   

    

     

    
    

     

      			}
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     
			// 
     
    
   

    

     

    
    

     

      			geometry_msgs::Twist vel_msg3;
     
    
   

    

     

    
    

     

      			vel_msg3.angular.z = 4.0 * atan2(transform3.getOrigin().y(),
     
    
   

    

     

    
    

     

      						                transform3.getOrigin().x());
     
    
   

    

     

    
    

     

      			vel_msg3.linear.x = 2.0 * sqrt(pow(transform3.getOrigin().x(), 2) +
     
    
   

    

     

    
    

     
						              pow(transform3.getOrigin().y(), 2));
     
    
   

    

     

    
    

     

      			turtle_vel3.publish(vel_msg3);
     
    
   

    

     

    
    

     

      		}
     
    
   

    

     

    
    

     
 
     
    
   

    

     

    
    

     

      		rate.sleep();
     
    
   

    

     

    
    

     

      	}
     
    
   

    

     

    
    

     
	return 0;
     
    
   

    

     

    
    

     

      };
     
    
  
 

文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/124205271