启智CV-小车灰度巡线

源码

#include <ros/ros.h>
#include <std msgs/Int32MultiArray.h>

#include <geometrymsgs Twist.h>

geometry msgs::Twist vel cmd;

static int araDVal[15];

void AD Callback(const std msgs::Int32MultiArraymsg)

for(int i=0;i<15;i++){
arADVal[i]=msg.data[i];
}
int main(int argc, char** argv)

{
ros::init(argc, argv,"path_following”); ros::NodeHandle n:
ros::Subscriber sub ad=n.subscribe("/wpbcv/ad"，100AD_Callback);
ros::Publisher vel_pub=n.advertise<geometry_msgs::Twist>(“/cmd_vel"，10);
ros::Rate loop rate(30); int nCountToStop=0; while(ros::ok()){
printf("[AD]");
for(int i=5;i<7;i++)
printf(”ad%d=%d ",i+1,arADVal[i]);}
printf("\n");
vel_cmd.linear.x=0.05; vel_cmd.angular.z=0; if(arADVa1[5]<1000){
vel_cmd. linear.x=0.05; vel_cmd.angular.z=0.1;
if(arADVal[6]<1000)
vel_cmd.linear.x=0.05; vel_cmd.angular.z=-0.1;
nCountToStop ++;
if(nCountToStop>300)
(
vel_cmd.linear.x=0; vel_cmd.angular.z=0; ROS_WARN("Stop!");

vel_pub.publish(vel_cmd); ros::spinOnce(); loop_rate.sleep();
return 0;