/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-16 https://www.robotway.com/

  ------------------------------*/

/*

*Control single bus steering grar

*

*

///////////////////////////

*        head

* ID:001       ID:000

*    ___________

*   |           |

*   |           |

*   |           |

*   |           |

*   |           |

*   |           |

*   |___________|

* ID:003       ID:002   

*      Rear end

*

* 2020.8.7 by boris

//////////////////////////

*/

#define mySerial Serial2           //Serial port used by bus steering gear

#define Serial_Baud_Rate 115200    //Serial Port baud rate

#define BusServoSerialBaud 115200   //Bus steering gear baud rate

#define wheel_speed_forward 0.08   //wheel forward speed

#define wheel_speed_back -0.08     //wheel back speed

#define wheel_speed_stop 0.0       //wheel stop speed

enum{FORWARD_LEFT=1,FORWARD_RIGHT,BACK_LEFT,BACK_RIGHT,STOP};//wheel named

void setup(){

  delay(1100);Serial.begin(Serial_Baud_Rate);

  mySerial.begin(BusServoSerialBaud);delay(1000);

}

void loop(){

  control_bus_steering_gear_test();// control single bus steering gear

}

void control_bus_steering_gear_test()

{

  Car_Move(FORWARD_LEFT,wheel_speed_forward);delay(1000);Stop_car();   //left front wheel forward

  Car_Move(FORWARD_LEFT,wheel_speed_back);delay(1000);Stop_car();      //left front wheel backword

  Car_Move(FORWARD_RIGHT,wheel_speed_forward);delay(1000);Stop_car();   //right front wheel forward

  Car_Move(FORWARD_RIGHT,wheel_speed_back);delay(1000);Stop_car();     //right front wheel backword

  Car_Move(BACK_LEFT,wheel_speed_forward);delay(1000);Stop_car();      //left rear wheel forward

  Car_Move(BACK_LEFT,wheel_speed_back);delay(1000);Stop_car();         //left rear wheel backword

  Car_Move(BACK_RIGHT,wheel_speed_forward);delay(1000);Stop_car();     //right rear wheel backword

  Car_Move(BACK_RIGHT,wheel_speed_back);delay(1000);Stop_car();        //right rear wheel backword

}

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-16 https://www.robotway.com/

  ------------------------------*/

/*

* car action test (include:forward, back, turnleft,turnright, left translation, right translation)

*                         

*

* IP address of bus steering gear

*        head

* ID:001       ID:000

*    ___________

*   |           |

*   |           |

*   |           |

*   |           |

*   |           |

*   |           |

*   |___________|

* ID:003       ID:002   

*      Rear end

*

* 2020.8.7 by boris

////////////////////////////////

*/

#define ActionDelayTimes 1000

#define mySerial Serial2

#define BusServoSerialBaud 115200

#define wheel_speed_forward 0.08    //car forward speed

#define wheel_speed_back -0.08      //car back speed

#define wheel_speed_stop 0.0        //car stop speed

#define wheel_speed_left 0.08       //car turnleft speed

#define wheel_speed_right -0.08     //car turnright speed

#define wheel_speed_left_translation 0.08   //speed of car left translation

#define wheel_speed_right_translation -0.08 //speed of car right translation

enum{FORWARD=1,BACK,LEFT,RIGHT,LEFT_TRANSLATION,RIGHT_TRANSLATION,STOP}; //the movement state of the car

float wheel_Speed[4]={0,0,0,0};

char cmd_return[200];

void setup()

{

  delay(1000);

  Serial.begin(57600);delay(1000);//open serial

  mySerial.begin(BusServoSerialBaud);delay(1000);//open serial2

  car_move_test();

}

void loop()

{

   //car_move_test();//car action test

   //car_stop();

}

void car_move_test()//car action test

{

  Car_Move(FORWARD);delay(ActionDelayTimes);          // forward

  car_stop();

  Car_Move(BACK);delay(ActionDelayTimes);             // back

  car_stop();

  Car_Move(LEFT_TRANSLATION);delay(ActionDelayTimes); //left translation

  car_stop();

  Car_Move(RIGHT_TRANSLATION);delay(ActionDelayTimes);//right translation

  car_stop();

  Car_Move(LEFT);delay(ActionDelayTimes);             //turnleft

  car_stop();

  Car_Move(RIGHT);delay(ActionDelayTimes);            //turn right

  car_stop();  

}

序号

程序源代码

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-16 https://www.robotway.com/

  ------------------------------*/

/*

*Tracking Car

*

*        head

* pin:42      pin:44

*    ___________

*   |           |

*   |           |

*   |           |

*   |           |

*   |___________|

* pin:36      pin:34   

*      Rear end

* 2020.8.7 by boris

*/

#define mySerial Serial2         //statement serial2

#define Sensor_Numbers 4         //numbers of sensor

#define Serial_Baud_Rate 115200   //serial port baud rate

#define BusServoSerialBaud 115200//bus steering gear baud rate

#define wheel_speed_forward 0.06 //car forward speed

#define wheel_speed_back -0.06   //car back speed

#define wheel_speed_stop 0.0     //car stop speed

#define wheel_speed_left 0.1    //car turnleft speed

#define wheel_speed_right -0.1   //car turnright speed

enum{FORWARD=1,BACK,LEFT,RIGHT,STOP};//the movement state of the car

const int Gray_Sensor_Pin[Sensor_Numbers] = {44,42,36,34};//define gray sensor pin

const int GraySensorNumbers = sizeof(Gray_Sensor_Pin)/sizeof(Gray_Sensor_Pin[0]);

char cmd_return[200];

void setup(){

  delay(1100);Serial.begin(Serial_Baud_Rate);

  Gray_Sensor_Init();

  mySerial.begin(BusServoSerialBaud);delay(1000);

}

void loop(){

  tracking_car();//car tracking

  //Car_Move(STOP);

}

void Gray_Sensor_Init()//sensor init

{

  for(int i=0;i<Sensor_Numbers;i++)

  {

     pinMode(Gray_Sensor_Pin[i],INPUT);delay(2);

  } delay(200);

}

void tracking_car() //car tracking

{

  int sensor_data[Sensor_Numbers] = {0,0,0,0};

  int sum = 0;

  for(int i=0;i<Sensor_Numbers-2;i++)

  {

     sensor_data[i] = digitalRead(Gray_Sensor_Pin[i]);

     sum |= sensor_data[i] << i;

  }  

  switch(sum)

  {

    case 0: Car_Move(FORWARD); break;

    case 1: Car_Move(LEFT); break;

    case 2: Car_Move(RIGHT); break;

    case 3: Car_Move(FORWARD); break;

    default: break;

  }

}

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-16 https://www.robotway.com/

  ------------------------------*/

/*

* Avoid obstacles car

*

*

*   车身位置及传感器接线:

*

* // Y

* // |

* // |

* // |              t:53                   t:8  

* // |     (ID:003) e:A15                  e:9   (ID:001)

* // |   t:37    A - - - - - - - - - - - - - - - - - -Y: t:39

* // |   e:38    |                                    |   e:40

* // |          |                                    |         车头

* // |   t:31    |                                    |   t:45

* // |   e:32    Z - - - - - - - - - - - - - - - - - -X: e:46

* // |     (ID:002) t:47                   t:A13   (ID:000)

* // |              e:48                   e:A14

* // 0-- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- X

* ////////////////////////////////////////////////////////////////////////////////////////////////

*  

* 2020.8.7 by boris

*/

#define mySerial Serial2            //Serial port used by bus steering gear           

#define Serial_Baud_Rate 115200     //Serial Port baud rate

#define BusServoSerialBaud 115200   //Bus steering gear baud rate

#define wheel_speed_forward 0.04    //car forward speed

#define wheel_speed_forward_up 0.06 //forward speed up

#define wheel_speed_back -0.04      //car back speed

#define wheel_speed_stop 0.0        //car stop speed

#define wheel_speed_left 0.1      //car turnleft speed

#define wheel_speed_right -0.1    //car turnright speed

#define wheel_speed_left_translation 0.04   //speed of car left translation

#define wheel_speed_right_translation -0.04 //speed of car right translation

enum{FORWARD=1,BACK,LEFT,RIGHT,LEFT_TRANSLATION,RIGHT_TRANSLATION,STOP,FORWARD_UP}; //the movement state of the car

char cmd_return[200];

float wheel_Speed[4]={0,0,0,0};

void setup(){

  delay(1100);Serial.begin(Serial_Baud_Rate);

  init_Ultrasonic();delay(200);

  mySerial.begin(BusServoSerialBaud);delay(1000);

}

void loop(){

  //Car_Move(STOP);delay(1000);

  car_move();

  //get_ultrasonic_sensor_data();

}

序号

程序源代码

#!/usr/bin/env python

#!-*-coding=utf-8 -*-

import rospy

from sensor_msgs.msg import Image

import cv2

import numpy as np

from cv_bridge import CvBridge, CvBridgeError

import sys

import time

lower_blue=np.array([100,43,46])

upper_blue=np.array([124,255,255])

#lower_blue=np.array([14,143,80])

#upper_blue=np.array([23,255,200])

#lower_red=np.array([0,200,55])

#upper_red=np.array([10,255,130])

#lower_red=np.array([0,150,55])

#upper_red=np.array([10,255,130])

lower_red=np.array([0,43,46])

upper_red=np.array([10,255,255])

lower_green=np.array([40,43,46])

upper_green=np.array([77,255,255])

font = cv2.FONT_HERSHEY_SIMPLEX   # 设置字体样式

kernel = np.ones((5, 5), np.uint8)   # 卷积核

img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)

#cap = cv2.VideoCapture(0)

def areaCal(contour):

    area = 0

    for i in range(len(contour)):

        area += cv2.contourArea(contour[i])

    return area

def image_callback(msg):

           bridge = CvBridge()

           frame = bridge.imgmsg_to_cv2(msg, "bgr8")

           #cv2.imshow("source", frame)

           #ret, frame = Video.read()

           gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)   # 转换为灰色通道

           hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)   # 转换为HSV空间

           mask = cv2.inRange(hsv, lower_red, upper_red)   # 设定掩膜取值范围   [消除噪声]

           #mask = cv2.inRange(hsv, lower_green, upper_green)   # 设定掩膜取值范围 [消除噪声]

           opening = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)   # 形态学开运算 [消除噪声]

           bila = cv2.bilateralFilter(mask, 10, 200, 200)   # 双边滤波消除噪声 [消除噪声]

           edges = cv2.Canny(opening, 50, 100)   # 边缘识别 [消除噪声]

           mask_green = cv2.inRange(hsv,lower_green,upper_green)

           opening_green = cv2.morphologyEx(mask_green, cv2.MORPH_OPEN, kernel)

           bila_green = cv2.bilateralFilter(mask_green, 10, 200, 200)

           edges_green = cv2.Canny(opening_green, 50, 100)

           images,contourss,hierarchvs = cv2.findContours(edges_green,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

           image,contours,hierarchv = cv2.findContours(edges,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

           scaling_factor = 0.5

           print "area_red=",areaCal(contours),"area_green=",areaCal(contourss)

           if(areaCal(contours)>50):

             #circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=20, minRadius=20, maxRadius=500)

             circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=20, minRadius=0, maxRadius=0)

             if circles is not None:   # 如果识别出圆

               #print "I found the red circle"

               for circle in circles[0]:

                 x_red=int(circle[0])

                 y_red=int(circle[1])

                 r_red=int(circle[2])

                 cv2.circle(frame, (x_red, y_red), r_red, (0, 0, 255), 3)   # 标记圆

                 cv2.circle(frame, (x_red, y_red), 3, (255, 255, 0), -1)   # 标记圆心

                 text = 'circle' + 'x:   '+str(x_red)+' y:   '+str(y_red)

                 cv2.putText(frame, text, (10, 30), font, 1, (0, 255, 0), 2, cv2.LINE_AA, 0)   # 显示圆心位置

           if(areaCal(contourss)>1000):

             #circles = cv2.HoughCircles(edges, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=20, minRadius=20, maxRadius=500)

             circles = cv2.HoughCircles(edges_green, cv2.HOUGH_GRADIENT, 1, 100, param1=100, param2=20, minRadius=0, maxRadius=0)

             if circles is not None:   # 如果识别出圆

               #print "I found the green circle"

               for circle in circles[0]:

                 x_red=int(circle[0])

                 y_red=int(circle[1])

                 r_red=int(circle[2])

                 cv2.circle(frame, (x_red, y_red), r_red, (0, 0, 255), 3)   # 标记圆

                 cv2.circle(frame, (x_red, y_red), 3, (255, 255, 0), -1)   # 标记圆心

                 text = 'circle' + 'x:   '+str(x_red)+' y:   '+str(y_red)

                 cv2.putText(frame, text, (10, 60), font, 1, (0, 255, 0), 2, cv2.LINE_AA, 0)   # 显示圆心位置

           frame = cv2.resize(frame,None,fx=scaling_factor,fy=scaling_factor,interpolation=cv2.INTER_AREA)

           msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")

           img_pub.publish(msg)

           cv2.waitKey(3)

           '''

           rate = rospy.Rate(20) # 5hz

           scaling_factor = 0.5

           bridge = CvBridge()

           count = 0

           while not rospy.is_shutdown():

             if (True):

               count = count + 1

             else:

               rospy.loginfo("Capturing image failed.")

             if count == 2:

               count = 0

               frame = cv2.resize(frame,None,fx=scaling_factor,fy=scaling_factor,interpolation=cv2.INTER_AREA)

               msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")

               img_pub.publish(msg)

           rate.sleep()         

           '''

def webcamImagePub():

    rospy.init_node('webcam_puber', anonymous=True)

#    img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)

    rospy.Subscriber("/image_raw", Image, image_callback)

    rospy.spin()

if __name__ == '__main__':

    try:

        webcamImagePub()

    except rospy.ROSInterruptException:

        pass

    finally:

        webcamImagePub()

#!/usr/bin/env python

#!-*-coding=utf-8 -*-

import rospy

from sensor_msgs.msg import Image

from cv_bridge import CvBridge, CvBridgeError

import cv2

import numpy as np

import serial

import time

import sys

from std_msgs.msg import UInt16

from std_msgs.msg import String

img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)

lower_blue=np.array([100,43,46])

upper_blue=np.array([124,255,255])

lower_red=np.array([0,43,46])

upper_red=np.array([10,255,255])

lower_green=np.array([40,43,46])

upper_green=np.array([77,255,255])

font = cv2.FONT_HERSHEY_SIMPLEX

kernel = np.ones((5, 5), np.uint8)

def areaCal(contour):

    area = 0

    for i in range(len(contour)):

        area += cv2.contourArea(contour[i])

    return area

def image_callback(msg):

  bridge = CvBridge()

  img = bridge.imgmsg_to_cv2(msg, "bgr8")

  #cv2.imshow("source", img)

  hsv = cv2.cvtColor(img,cv2.COLOR_BGR2HSV)

  mask_red = cv2.inRange(hsv,lower_red,upper_red)

  res = cv2.bitwise_and(img,img,mask=mask_red)

  scaling_factor = 0.5

#   cv2.imshow("res",res)

  image,contours,hierarchv = cv2.findContours(mask_red,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

#   print   "red=",areaCal(contours)

  if (areaCal(contours)>5000):

#    print "red color"

    text_red = 'the color is red'

    cv2.putText(img, text_red, (10, 30), font, 1, (0, 0, 255), 2, cv2.LINE_AA, 0)

  mask_blue= cv2.inRange(hsv, lower_blue, upper_blue)

  res_yellow = cv2.bitwise_and(img,img,mask=mask_blue)

  image,contours,hierarchv = cv2.findContours(mask_blue,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

  if(areaCal(contours)>8000):

    text_blue = 'the color is blue'

#    print "blue color"

    cv2.putText(img, text_blue, (10, 60), font, 1, (255, 0, 0), 2, cv2.LINE_AA, 0)

  mask_green=cv2.inRange(hsv, lower_green,upper_green)

  res_green = cv2.bitwise_and(img,img,mask=mask_blue)

  image,contours,hierarchv = cv2.findContours(mask_green,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)

  if (areaCal(contours)>5000):

    text_green = 'the color is green'

    cv2.putText(img, text_green, (10, 90), font, 1, (0, 255, 0), 2, cv2.LINE_AA, 0)

#    print ("green color")

#   else:

#     print( "NONE COLOR" )

  frame = cv2.resize(img,None,fx=scaling_factor,fy=scaling_factor,interpolation=cv2.INTER_AREA)

  msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")

  img_pub.publish(msg)

  cv2.waitKey(3)

#   cv2.waitKey(1)

def main():

    rospy.init_node('image_listener',anonymous=True)

    #image_topic = "/camera/color/image_raw"

    #rospy.Subscriber("/camera/color/image_raw", Image, image_callback)

    rospy.Subscriber("/image_raw", Image, image_callback)

    rospy.spin()

if __name__ == '__main__':

    main()

#!/usr/bin/env python

#!coding=utf-8

import rospy

from sensor_msgs.msg import Image

from cv_bridge import CvBridge, CvBridgeError

import cv2

import numpy as np

import serial

import time

import sys

from std_msgs.msg import UInt16

from std_msgs.msg import String

#lower_blue=np.array([50,143,146])

#upper_blue=np.array([124,255,255])

#lower_blue=np.array([14,143,80])

#upper_blue=np.array([23,255,200])

lower_blue=np.array([100,43,46])

upper_blue=np.array([124,255,255])

#lower_red=np.array([0,200,55])

#upper_red=np.array([10,255,130])

#lower_red=np.array([0,150,55])

#upper_red=np.array([10,255,130])

lower_red=np.array([0,43,46])

upper_red=np.array([10,255,255])

lower_green=np.array([40,43,46])

upper_green=np.array([77,255,255])

#ser = serial.Serial('/dev/ttyACM0', 57600, timeout=2.0) #2020.8.28 source value=0.5

img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)

data_pub= rospy.Publisher('Color_center_point',String,queue_size=20)

red_flag=0

blue_flag=0

image_flags = 1

cx_string=""

cy_string=""

cx_cy_string=""

send_data = ""

count=0

scaling_factor = 1.0   #2020.8.28 source value=0.5

def areaCal(contour):

    area = 0

    for i in range(len(contour)):

        area += cv2.contourArea(contour[i])

    return area

def image_callback(msg):

  global count,send_data

  bridge = CvBridge()

  frame = bridge.imgmsg_to_cv2(msg, "bgr8")

  #cv2.imshow("source", frame)

  hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)

  mask_red = cv2.inRange(hsv,lower_red,upper_red)

  res = cv2.bitwise_and(frame,frame,mask=mask_red)

  #cv2.imshow("res",res)

  image,contours,hierarchv = cv2.findContours(mask_red,cv2.RETR_TREE,cv2.CHAIN_APPROX_SIMPLE)  

  print "mianji=",areaCal(contours)

  if (areaCal(contours)>5000):

    if(areaCal(contours)>80000):

      send_data="back"

      #ser.write("back\n")

      #print "back"

    elif( areaCal(contours)<50000 and areaCal(contours)>5000 ):

      send_data="forward"

      #ser.write("forward\n")

      #print "forward"

    else:

      send_data="state"

    if len(contours) > 0:

      c = max(contours, key=cv2.contourArea)

      #print "c=",c

      cnt=contours[0]

      cv2.drawContours(frame, c, -1, (0, 0, 255), 3)#画轮廓

      #cv2.imshow("drawcontours",frame)

      M = cv2.moments(c)

      if M["m00"] !=0:

        cx = int(M['m10']/M['m00'])

        cy = int(M['m01']/M['m00'])

        center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

        cv2.circle(frame, (cx,cy), 8, (0, 255, 0), -1)

        print "center=",center,"cx=",cx,"cy=",cy

        cx_string=str(cx)

        cy_string=str(cy)

        #cx_cy_string=(cx_string + "-" + cy_string + "\n").encode('utf-8')

        #cx_cy_string=(cx_string + "-" + cy_string + "-" + send_data + "\n").encode('utf-8')

        cx_cy_string=(cx_string + "-" + cy_string + "+" + send_data ).encode('utf-8')

        print (cx_cy_string)

        data_pub.publish(cx_cy_string)

      else:

        cx=0

        cy=0

    else:

      print "no red color something"

      #cv2.imshow("chanle", img)

#      if cv2.waitKey(1) & 0xFF == ord('q'):

#        #break

#        #continue

  if(image_flags==1):

    count = count+1

  else:

    rospy.loginfo("Capturing image failed.")

  if count == 2:

    count = 0

    frame = cv2.resize(frame,None,fx=scaling_factor,fy=scaling_factor,interpolation=cv2.INTER_AREA)

    msg = bridge.cv2_to_imgmsg(frame, encoding="bgr8")

    img_pub.publish(msg)

  cv2.waitKey(1)

def main():

    rospy.init_node('image_listener',anonymous=True)

#    img_pub = rospy.Publisher('webcam/image_raw', Image, queue_size=2)

    rospy.Subscriber("/image_raw", Image, image_callback)

    rospy.spin()

if __name__ == '__main__':

    main()

/*

* rosserial Publisher Example

* Prints "hello world!"

*/

#include <ros.h>

#include <std_msgs/String.h>

#include <Arduino.h>

#include <stdio.h>

#include <string.h>

#define CTL_BAUDRATE 115200 //总线舵机波特率

#define mySerial Serial2

#define SerialBaudrate 57600

#define RGB_LED_NUMBERS 3

#define Bus_servo_Angle_inits 1500

#define ActionDelayTimes 1500

//

#define wheel_speed_forward 0.08    //car forward speed

#define wheel_speed_back -0.08      //car back speed

#define wheel_speed_stop 0.0        //car stop speed

#define wheel_speed_left 0.1       //car turnleft speed

#define wheel_speed_right -0.1     //car turnright speed

#define wheel_speed_left_translation 0.08   //speed of car left translation

#define wheel_speed_right_translation -0.08 //speed of car right translation

char cmd_return[200];

String receive_string="hello";

ros::NodeHandle   nh;

void messageCb( const std_msgs::String &toggle_msg){

   receive_string=toggle_msg.data;

}

ros::Subscriber<std_msgs::String> sub("Color_center_point", &messageCb );

enum{FORWARD=1,BACK,LEFT,RIGHT,LEFT_TRANSLATION,RIGHT_TRANSLATION,STOP}; //the movement state of the car

void setup()

{

  delay(1100);

  Serial.begin(SerialBaudrate);

  mySerial.begin(CTL_BAUDRATE);  

  nh.initNode();

  nh.subscribe(sub);

}

void loop()

{

  if( (receive_string.length())<5 && (receive_string.length())>15 )

  {

     for(int i=0;i<1;i++){

     break;

     }

  }

    else{

       int SpacePosition[2] = {0,0};

       int Data_one = 0;

       int Data_two = 0;

       int numbers_left=0 ,numbers_right=0;

       char num_left[32] = {};

       char num_right[32] = {};

       String x_data="";

       String y_data="";

       String z_data="";

       String new_string = "";                                                                               

       SpacePosition[0] = receive_string.indexOf('-');

       x_data = receive_string.substring(0,SpacePosition[0]);

       //if(x_data.length()>=4){break;}

       new_string = receive_string.substring(SpacePosition[0]+1);

       SpacePosition[1] = new_string.indexOf('+');

       y_data = new_string.substring(0,SpacePosition[1]);

       z_data = new_string.substring(SpacePosition[1]+1);

      Data_one = x_data.toInt();

      Data_two = y_data.toInt();

      //if( (Data_one<=120) && (z_data =="state") ){Car_Move(LEFT_TRANSLATION);}

      if( (Data_one<=280) && (Data_one>=20)){Car_Move(LEFT_TRANSLATION);}

      else if ( (Data_one>=360) && (Data_one<=600) ){Car_Move(RIGHT_TRANSLATION);}

      else if( z_data == "forward" ){Car_Move(FORWARD);}

      else if( z_data == "back" ){Car_Move(BACK );}

      else {Car_Move(STOP);}

      receive_string = "";

      x_data="";

      y_data="";

      z_data="";

      new_string="";

    }  

  nh.spinOnce();

  delay(100);

}

序号

程序源代码

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-22 https://www.robotway.com/

  ------------------------------*/

//DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要

//#define USE_USBCON          //PAOGRAMMING PORT

//#define USE_NATIVE_USB      //NATIVE USB PORT

#define ActionDelayTimes 1500

//#include <SoftwareSerial.h>

#include <ros.h>

#include <ros/time.h>

//#include<ServoTimer2.h>

#include <geometry_msgs/Vector3.h>

#define mySerial Serial2

#define BusServoSerialBaud 115200

ros::NodeHandle   nh;

void XYRun(double vx, double vy, double w);

void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}

ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );

geometry_msgs::Vector3 pose_message;

ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);

geometry_msgs::Vector3 vel_message;

ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);

//ServoTimer2 myServo[4];//statement servo

//int servo_port[4] = {10, 11, 12, 13};//x,y,z,a

//float value_init[4] = {88, 88, 96, 86};//x,y,z,a

const int kMessagePubRate = 5;

unsigned long message_pub_time = 0;

const int kReadMotorDeltaT = 5;

unsigned long position_read_time = 0;

float current_x = 0,current_y = 0,current_a = 0;

float current_vx = 0,current_vy = 0,current_va = 0;

float wheel_Speed[4]={0,0,0,0};

char cmd_return[200];

float sudu = 0.87;

//SoftwareSerial mySerial(51, 9);

void setup()

{

  delay(1000);

  Serial.begin(57600);delay(1000);

  mySerial.begin(BusServoSerialBaud);delay(1000);

  //Servo_Action_Test();delay(1500);  

  nh.initNode();

  nh.subscribe(vel_cmd_sub);

  nh.advertise(pose_feedback_pub);

  nh.advertise(vel_feedback_pub);

  XYSetVel(0.0,0.0,0.0);

  position_read_time = millis();

  message_pub_time = millis();

}

void loop()

{

//   unsigned long times = millis();

//   set_Single_servo_Vel(1600);delay(5000);

//   Serial.println(millis()-times);

//   set_Single_servo_Vel(1500);

//   while(1){

//    delay(100);

//   }

  if(millis()>position_read_time)

  {  

    XYread();

    position_read_time+=kReadMotorDeltaT;

  }  

  if(millis()>message_pub_time)

  {

    pose_message.x = current_x;

    pose_message.y = current_y;

    pose_message.z = current_a;

    vel_message.x = current_vx;

    vel_message.y = current_vy;

    vel_message.z = current_va;

    pose_feedback_pub.publish(&pose_message);

    vel_feedback_pub.publish(&vel_message);

    message_pub_time+=1000.0/kMessagePubRate;

  }

  nh.spinOnce();

}

/*------------------------------------------------------------------------------------

  版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-08-22 https://www.robotway.com/

  ------------------------------*/

//DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要

//#define USE_USBCON          //PAOGRAMMING PORT

//#define USE_NATIVE_USB      //NATIVE USB PORT

#define ActionDelayTimes 1500

//#include <SoftwareSerial.h>

#include <ros.h>

#include <ros/time.h>

//#include<ServoTimer2.h>

#include <geometry_msgs/Vector3.h>

#define mySerial Serial2

#define BusServoSerialBaud 115200

ros::NodeHandle   nh;

void XYRun(double vx, double vy, double w);

void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}

ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );

geometry_msgs::Vector3 pose_message;

ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);

geometry_msgs::Vector3 vel_message;

ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);

//ServoTimer2 myServo[4];//statement servo

//int servo_port[4] = {10, 11, 12, 13};//x,y,z,a

//float value_init[4] = {88, 88, 96, 86};//x,y,z,a

const int kMessagePubRate = 5;

unsigned long message_pub_time = 0;

const int kReadMotorDeltaT = 5;

unsigned long position_read_time = 0;

float current_x = 0,current_y = 0,current_a = 0;

float current_vx = 0,current_vy = 0,current_va = 0;

float wheel_Speed[4]={0,0,0,0};

char cmd_return[200];

float sudu = 0.87;

//SoftwareSerial mySerial(51, 9);

void setup()

{

  delay(1000);

  Serial.begin(57600);delay(1000);

  mySerial.begin(BusServoSerialBaud);delay(1000);

  //Servo_Action_Test();delay(1500);  

  nh.initNode();

  nh.subscribe(vel_cmd_sub);

  nh.advertise(pose_feedback_pub);

  nh.advertise(vel_feedback_pub);

  XYSetVel(0.0,0.0,0.0);

  position_read_time = millis();

  message_pub_time = millis();

}

void loop()

{

//   unsigned long times = millis();

//   set_Single_servo_Vel(1600);delay(5000);

//   Serial.println(millis()-times);

//   set_Single_servo_Vel(1500);

//   while(1){

//    delay(100);

//   }

  if(millis()>position_read_time)

  {  

    XYread();

    position_read_time+=kReadMotorDeltaT;

  }  

  if(millis()>message_pub_time)

  {

    pose_message.x = current_x;

    pose_message.y = current_y;

    pose_message.z = current_a;

    vel_message.x = current_vx;

    vel_message.y = current_vy;

    vel_message.z = current_va;

    pose_feedback_pub.publish(&pose_message);

    vel_feedback_pub.publish(&vel_message);

    message_pub_time+=1000.0/kMessagePubRate;

  }

  nh.spinOnce();

}

序号

程序源代码