为第五期新增imu红外代码

src/robot_demo/CMakeLists.txt

@@ -0,0 +1,223 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(robot_demo)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  cv_bridge
+  ros_arduino_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES robot_demo
+#  CATKIN_DEPENDS roscpp stdmsgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/robot_demo.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+
+add_executable(${PROJECT_NAME}_imu_node src/imu_demo_node.cpp)
+add_executable(${PROJECT_NAME}_infrared_node src/infrared_demo_node.cpp)
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME}_infrared_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+target_link_libraries(${PROJECT_NAME}_imu_node
+  ${catkin_LIBRARIES}
+)
+
+target_link_libraries(${PROJECT_NAME}_infrared_node
+   ${catkin_LIBRARIES}
+)
+
+
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_demo.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/robot_demo/cfg/param.yaml

@@ -0,0 +1,10 @@
+#####################################################
+# Copyright: 2016-2021 www.corvin.cn ROS小课堂
+# Description:测试红外小程序
+#   center_dist:红外测距时悬停的距离.
+# Author: adam_zhuo
+# History:
+#   20210512:init this file.
+######################################################
+
+center_dist: 0.15

src/robot_demo/include/imu_demo.h

@@ -0,0 +1,36 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-30 15:29:44
+ * @Description: imu小程序头文件
+ * @History: 20210430:初始化文件-adam_zhuo
+ */
+#ifndef _IMU_DEMO_H
+#define _IMU_DEMO_H
+
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <geometry_msgs/Twist.h>
+
+class Imu_Demo
+{
+private:
+    bool start_flag;
+    int target_angle;
+    float target_radian;
+    float current_radian;
+    float target_radian_copy;
+    float current_radian_copy;
+    ros::NodeHandle nh;
+    ros::Subscriber _angle_sub;
+    ros::Subscriber _yaw_sub;
+    ros::Publisher _cmd_vel_pub;
+    geometry_msgs::Twist twist;
+public:
+    Imu_Demo();
+    ~Imu_Demo();
+    void angle_callback(const std_msgs::Float32::ConstPtr &data);
+    void yaw_callback(const std_msgs::Float32::ConstPtr &data);
+};
+
+#endif

src/robot_demo/include/infrared_demo.h

@@ -0,0 +1,29 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 09:48:43
+ * @Description: 红外demo头文件
+ * @History: 20210512:初始化文件-adam_zhuo
+ */
+#ifndef _INFRARED_DEMO_H
+#define _INFRARED_DEMO_H
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include "ros_arduino_msgs/InfraredSensors.h"
+
+class Infrared_Demo
+{
+private:
+    ros::NodeHandle nh;
+    ros::Subscriber _infrared_sub;
+    ros::Publisher _cmd_vel_pub;
+    geometry_msgs::Twist _twist;
+    float center_dist;
+public:
+    Infrared_Demo();
+    ~Infrared_Demo();
+    void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr &data);
+};
+
+#endif

src/robot_demo/launch/imu_demo.launch

@@ -0,0 +1,13 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 11:16:35
+ * @Description: 启动imu程序节点
+ * @History: 20210512:初始化文件-adam_zhuo
+-->
+<launch>
+  <!-- 启动robot_demo_imu_node -->
+  <node pkg="robot_demo" type="robot_demo_imu_node" name="robot_demo_imu_node" output="screen">
+
+  </node>
+</launch>

src/robot_demo/launch/infrared_demo.launch

@@ -0,0 +1,16 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-12 09:53:40
+ * @Description: 启动红外demo节点
+ * @History: 20210429:初始化文件-adam_zhuo
+-->
+<launch>
+  <!-- 加载参数 -->
+  <arg name="cfg_file" default="$(find robot_demo)/cfg/param.yaml" />
+
+  <!-- 启动robot_demo_infrared_node -->
+  <node pkg="robot_demo" type="robot_demo_infrared_node" name="robot_demo_infrared_node" output="screen">
+    <rosparam file="$(arg cfg_file)" command="load" />
+  </node>
+</launch>

src/robot_demo/launch/voice_demo.launch

@@ -0,0 +1,14 @@
+<!--
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-05-13 11:16:35
+ * @Description: 启动语音程序节点
+ * @History: 20210513:初始化文件-adam_zhuo
+-->
+
+<launch>
+  <!-- 启动robot_demo_voice_node -->
+  <node pkg="robot_demo" type="robot_demo_voice_node" name="robot_demo_voice_node" output="screen">
+
+  </node>
+</launch>

src/robot_demo/package.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_demo</name>
+  <version>0.0.0</version>
+  <description>The robot_demo package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="adam_zhuo@corvin.cn">adam_zhuo</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/robot_demo</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>ros_arduino_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>ros_arduino_msgs</build_export_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>ros_arduino_msgs</exec_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/robot_demo/src/imu_demo_node.cpp

@@ -0,0 +1,68 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-30 15:29:58
+ * @Description: imu小程序
+ * @History: 20210429:初始化文件-adam_zhuo
+ */
+
+#include "imu_demo.h"
+
+/**
+ * @description: 构造函数初始化变量。
+ */
+Imu_Demo::Imu_Demo(){
+    
+    start_flag = false;
+    _angle_sub = nh.subscribe<std_msgs::Float32>("/angle", 1, &Imu_Demo::angle_callback, this);
+    _yaw_sub = nh.subscribe<std_msgs::Float32>("/yaw_data", 1, &Imu_Demo::yaw_callback, this);
+    _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+}
+
+Imu_Demo::~Imu_Demo(){
+
+}
+
+/**
+ * @description: 接收指定角度回调函数。程序启动后,发送指定角度，任意放置小车，小车最终朝向都会是指定角度。
+ */
+void Imu_Demo::angle_callback(const std_msgs::Float32::ConstPtr &data){
+    start_flag = true;
+    target_radian = data->data/180*M_PI;
+    target_radian_copy = target_radian;
+    if(target_radian < 0)
+        target_radian += (M_PI * 2);
+}
+
+/**
+ * @description: yaw角回调函数，不断检测当前小车朝向与指定角度的差值，如果大于一个阈值，则控制小车旋转。
+ */
+void Imu_Demo::yaw_callback(const std_msgs::Float32::ConstPtr &data){
+
+    //如果没有接收到发送的指定角度，则不做任何处理直接return
+    if(!start_flag)
+        return;
+    
+    //获取当前yaw角值
+    current_radian = data->data ;
+    current_radian_copy = current_radian;
+
+    // >>>判断与指定角度的差值，控制小车旋转
+    
+
+    
+    // <<<判断与指定角度的差值，控制小车旋转
+
+    //向小车发送控制命令
+    _cmd_vel_pub.publish(twist);
+}
+
+int main(int argc, char **argv){
+
+    //初始化节点
+    ros::init(argc, argv, "imu_demo_node");
+    //根据Imu_Demo声明对象，调用构造函数
+    Imu_Demo imu_demo;
+    //使ROS中回调函数生效
+    ros::spin();
+}

src/robot_demo/src/infrared_demo_node.cpp

@@ -0,0 +1,48 @@
+/*
+ * @Copyright(C): 2016-2021 ROS小课堂 www.corvin.cn
+ * @Author: adam_zhuo
+ * @Date: 2021-04-29 11:24:02
+ * @Description: 实现Infrafed_Demo类，main函数入口
+ * @History: 20210429:初始化-adam_zhuo
+ */
+#include <infrared_demo.h>
+
+Infrared_Demo::Infrared_Demo(){
+
+    //从参数服务器中取“center_dist”数据
+    ros::param::param<float>("~center_dist", center_dist, 0.2);
+
+    //订阅红外数据
+    _infrared_sub = nh.subscribe<ros_arduino_msgs::InfraredSensors>("/mobilebase_arduino/sensor/GP2Y0A41", 1, &Infrared_Demo::infrared_callback, this);
+    //注册小车速度发布者
+    _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+}
+
+Infrared_Demo::~Infrared_Demo(){
+}
+
+/**
+ * @description: 订阅红外数据回调函数
+ */
+void Infrared_Demo::infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr &data){
+
+    //获取前红外信息
+    float front_dist = data->front_dist;
+
+    // >>>根据前红外反馈的距离来控制小车运动，离小车较远时小车前进，离小车较近时小车后退
+    
+
+
+    
+    // <<<根据前红外反馈的距离来控制小车运动，离小车较远时小车前进，离小车较近时小车后退
+}
+
+int main(int argc, char **argv){
+
+    //初始化节点
+    ros::init(argc, argv, "infrared_demo_node");
+    //根据Infrafed_Demo声明对象，调用构造函数
+    Infrared_Demo infrared_demo;
+    //使ROS中回调函数生效
+    ros::spin();
+}

src/ros_arduino_bridge/README.md

@@ -0,0 +1,495 @@
+Overview
+--------
+This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
+
+This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services.  The stack does **not** depend on ROS Serial.
+
+Features of the stack include:
+
+* Direct support for Ping sonar and Sharp infrared (GP2D12) sensors
+
+* Can also read data from generic analog and digital sensors
+
+* Can control digital outputs (e.g. turn a switch or LED on and off)
+
+* Support for PWM servos
+* Configurable base controller if using the required hardware
+
+The stack includes a base controller for a differential drive
+robot that accepts ROS Twist messages and publishes odometry data back to
+the PC. The base controller requires the use of a motor controller and encoders for reading odometry data.  The current version of the stack provides support for the following base controller hardware:
+
+* Pololu VNH5019 dual motor controller shield (http://www.pololu.com/catalog/product/2502) or Pololu MC33926 dual motor shield (http://www.pololu.com/catalog/product/2503).
+
+* Robogaia Mega Encoder shield
+(http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
+
+**NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega. The on-board wheel encoder counters are currently only supported by Arduino Uno.
+
+* The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
+
+Official ROS Documentation
+--------------------------
+A standard ROS-style version of this documentation can be found on the ROS wiki at:
+
+http://www.ros.org/wiki/ros_arduino_bridge
+
+
+System Requirements
+-------------------
+**Python Serial:** To install the python-serial package under Ubuntu, use the command:
+
+    $ sudo apt-get install python-serial
+
+On non-Ubuntu systems, use either:
+
+    $ sudo pip install --upgrade pyserial
+
+or
+
+    $ sudo easy_install -U pyserial
+
+**Arduino IDE 1.6.6 or Higher:**
+Note that the preprocessing of conditional #include statements is broken in earlier versions of the Arduino IDE.  To ensure that the ROS Arduino Bridge firmware compiles correctly, be sure to install version 1.6.6 or higher of the Arduino IDE.  You can download the IDE from https://www.arduino.cc/en/Main/Software.
+
+**Hardware:**
+The firmware should work with any Arduino-compatible controller for reading sensors and controlling PWM servos.  However, to use the base controller, you will need a supported motor controller and encoder hardware as described above. If you do not have this hardware, you can still try the package for reading sensors and controlling servos.  See the NOTES section at the end of this document for instructions on how to do this.
+
+To use the base controller you must also install the appropriate libraries for your motor controller and encoders.  For the Pololu VNH5019 Dual Motor Shield, the library can be found at:
+
+https://github.com/pololu/Dual-VNH5019-Motor-Shield
+
+For the Pololu MC33926 Dual Motor Shield, the library can be found at:
+
+https://github.com/pololu/dual-mc33926-motor-shield
+
+The Robogaia Mega Encoder library can be found at:
+
+http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
+
+These libraries should be installed in your standard Arduino
+sketchbook/libraries directory.
+
+Finally, it is assumed you are using version 1.0 or greater of the
+Arduino IDE.
+
+Preparing your Serial Port under Linux
+--------------------------------------
+Your Arduino will likely connect to your Linux computer as port /dev/ttyACM# or /dev/ttyUSB# where # is a number like 0, 1, 2, etc., depending on how many other devices are connected.  The easiest way to make the determination is to unplug all other USB devices, plug in your Arduino, then run the command:
+
+    $ ls /dev/ttyACM*
+
+or 
+
+    $ ls /dev/ttyUSB*
+
+Hopefully, one of these two commands will return the result you're looking for (e.g. /dev/ttyACM0) and the other will return the error "No such file or directory".
+
+Next you need to make sure you have read/write access to the port.  Assuming your Arduino is connected on /dev/ttyACM0, run the command:
+
+    $ ls -l /dev/ttyACM0
+
+and you should see an output similar to the following:
+
+    crw-rw---- 1 root dialout 166, 0 2013-02-24 08:31 /dev/ttyACM0
+
+Note that only root and the "dialout" group have read/write access.  Therefore, you need to be a member of the dialout group.  You only have to do this once and it should then work for all USB devices you plug in later on.
+
+To add yourself to the dialout group, run the command:
+
+    $ sudo usermod -a -G dialout your_user_name
+
+where your\_user\_name is your Linux login name.  You will likely have to log out of your X-window session then log in again, or simply reboot your machine if you want to be sure.
+
+When you log back in again, try the command:
+
+    $ groups
+
+and you should see a list of groups you belong to including dialout. 
+
+Installation of the ros\_arduino\_bridge Stack
+----------------------------------------------
+
+    $ cd ~/catkin_workspace/src
+    $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
+    $ cd ~/catkin_workspace
+    $ catkin_make
+
+The provided Arduino library is called ROSArduinoBridge and is
+located in the ros\_arduino\_firmware package.  This sketch is
+specific to the hardware requirements above but it can also be used
+with other Arduino-type boards (e.g. Uno) by turning off the base
+controller as described in the NOTES section at the end of this
+document.
+
+To install the ROSArduinoBridge library, follow these steps:
+
+    $ cd SKETCHBOOK_PATH
+
+where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory.
+
+    $ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge
+
+This last command copies the ROSArduinoBridge sketch files into your sketchbook folder.  The next section describes how to configure, compile and upload this sketch.
+
+
+Loading the ROSArduinoBridge Sketch
+-----------------------------------
+
+* If you are using the base controller, make sure you have already installed the appropriate motor controller and encoder libraries into your Arduino sketchbook/librariesfolder.
+
+* Launch the Arduino 1.0 IDE and load the ROSArduinoBridge sketch.
+  You should be able to find it by going to:
+
+    File->Sketchbook->ROSArduinoBridge
+  
+NOTE: If you don't have the required base controller hardware but
+still want to try the code, see the notes at the end of the file.
+
+Choose one of the supported motor controllers by uncommenting its #define statement and commenting out any others.  By default, the Pololu VNH5019 driver is chosen.
+
+Choose a supported encoder library by by uncommenting its #define statement and commenting out any others.  At the moment, only the Robogaia Mega Encoder shield is supported and it is chosen by default.
+
+If you want to control PWM servos attached to your controller, look for the line:
+
+<pre>
+#define USE_SERVOS
+</pre>
+
+and make sure it is not commented out like this:
+
+<pre>
+//#define USE_SERVOS
+</pre>
+
+You must then edit the include file servos.h and change the N_SERVOS
+parameter as well as the pin numbers for the servos you have attached.
+
+* Compile and upload the sketch to your Arduino.
+
+Firmware Commands
+-----------------
+The ROSArduinoLibrary accepts single-letter commands over the serial port for polling sensors, controlling servos, driving the robot, and reading encoders.  These commands can be sent to the Arduino over any serial interface, including the Serial Monitor in the Arduino IDE.
+
+**NOTE:** Before trying these commands, set the Serial Monitor baudrate to 57600 and the line terminator to "Carriage return" or "Both NL & CR" using the two pulldown menus on the lower right of the Serial Monitor window.
+
+The list of commands can be found in the file commands.h.  The current list includes:
+
+<pre>
+#define ANALOG_READ    'a'
+#define GET_BAUDRATE   'b'
+#define PIN_MODE       'c'
+#define DIGITAL_READ   'd'
+#define READ_ENCODERS  'e'
+#define MOTOR_SPEEDS   'm'
+#define PING           'p'
+#define RESET_ENCODERS 'r'
+#define SERVO_WRITE    's'
+#define SERVO_READ     't'
+#define UPDATE_PID     'u'
+#define DIGITAL_WRITE  'w'
+#define ANALOG_WRITE   'x'
+</pre>
+
+For example, to get the analog reading on pin 3, use the command:
+
+a 3
+
+To change the mode of digital pin 3 to OUTPUT, send the command:
+
+c 3 1
+
+To get the current encoder counts:
+
+e
+
+To move the robot forward at 20 encoder ticks per second:
+
+m 20 20
+
+
+Testing your Wiring Connections
+-------------------------------
+On a differential drive robot, the motors are connected to the motor controller terminals with opposite polarities to each other.  Similarly, the A/B leads from the encoders are connected in the reverse sense to each other.  However, you still need to make sure that (a) the wheels move forward when given a positive motor speed and (b) that the encoder counts increase when the wheels move forward.
+
+After **placing your robot on blocks**, you can use the Serial Monitor in the Arduino IDE to test both requirements.  Use the 'm' command to activate the motors, the 'e' command to get the encoder counts, and the 'r' command to reset the encoders to 0.  Remember that at the firmware level, motor speeds are given in encoder ticks per second so that for an encoder resolution of, say 4000 counts per wheel revolution, a command such as 'm 20 20' should move the wheels fairly slowly.  (The wheels will only move for 2 seconds which is the default setting for the AUTO\_STOP\_INTERVAL.)  Also remember that the first argument is the left motor speed and the second argument is the right motor speed.  Similarly, when using the 'e' command, the first number returned is the left encoder count and the second number is the right encoder count.
+
+Finally, you can use the 'r' and 'e' commands to verify the expected encoder counts by rotating the wheels by hand roughly one full turn and checking the reported counts.
+
+
+Configuring the ros\_arduino\_python Node
+-----------------------------------------
+Now that your Arduino is running the required sketch, you can
+configure the ROS side of things on your PC.  You define your robot's
+dimensions, PID parameters, and sensor configuration by editing the
+YAML file in the directory ros\_arduino\_python/config.  So first move
+into that directory:
+
+    $ roscd ros_arduino_python/config
+
+Now copy the provided config file to one you can modify:
+
+    $ cp arduino_params.yaml my_arduino_params.yaml
+
+Bring up your copy of the params file (my\_arduino\_params.yaml) in
+your favorite text editor.  It should start off looking like this:
+
+<pre>
+port: /dev/ttyUSB0
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: False
+base_controller_rate: 10
+
+# === Robot drivetrain parameters
+#wheel_diameter: 0.146
+#wheel_track: 0.2969
+#encoder_resolution: 8384 # from Pololu for 131:1 motors
+#gear_reduction: 1.0
+#motors_reversed: True
+
+# === PID parameters
+#Kp: 20
+#Kd: 12
+#Ki: 0
+#Ko: 50
+#accel_limit: 1.0
+
+# === Sensor definitions.  Examples only - edit for your robot.
+#     Sensor type can be one of the follow (case sensitive!):
+#	  * Ping
+#	  * GP2D12
+#	  * Analog
+#	  * Digital
+#	  * PololuMotorCurrent
+#	  * PhidgetsVoltage
+#	  * PhidgetsCurrent (20 Amp, DC)
+
+sensors: {
+  #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
+  #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
+  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
+  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
+  arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
+}
+</pre>
+
+**NOTE**: Do not use tabs in your .yaml file or the parser will barf it back out when it tries to load it.   Always use spaces instead.  **ALSO**: When defining your sensor parameters, the last sensor in the list does **not** get a comma (,) at the end of the line but all the rest **must** have a comma.
+
+Let's now look at each section of this file.
+
+ _Port Settings_
+
+The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0. Set accordingly.
+
+The MegaRobogaiaPololu Arudino sketch connects at 57600 baud by default.
+
+_Polling Rates_
+
+The main *rate* parameter (50 Hz by default) determines how fast the
+outside ROS loop runs.  The default should suffice in most cases.  In
+any event, it should be at least as fast as your fastest sensor rate
+(defined below).
+
+The *sensorstate\_rate* determines how often to publish an aggregated
+list of all sensor readings.  Each sensor also publishes on its own
+topic and rate.
+
+The *use\_base\_controller* parameter is set to False by default.  Set it to True to use base control (assuming you have the required hardware.)  You will also have to set the PID paramters that follow.
+
+The *base\_controller\_rate* determines how often to publish odometry readings.
+
+_Defining Sensors_
+
+The *sensors* parameter defines a dictionary of sensor names and
+sensor parameters. (You can name each sensor whatever you like but
+remember that the name for a sensor will also become the topic name
+for that sensor.)
+
+The four most important parameters are *pin*, *type*, *rate* and *direction*.
+The *rate* defines how many times per second you want to poll that
+sensor.  For example, a voltage sensor might only be polled once a
+second (or even once every 2 seconds: rate=0.5), whereas a sonar
+sensor might be polled at 20 times per second.  The *type* must be one
+of those listed (case sensitive!).  The default *direction* is input so
+to define an output pin, set the direction explicitly to output.  In
+the example above, the Arduino LED (pin 13) will be turned on and off
+at a rate of 2 times per second.
+
+_Setting Drivetrain and PID Parameters_
+
+To use the base controller, you will have to uncomment and set the
+robot drivetrain and PID parameters.  The sample drivetrain parameters
+are for 6" drive wheels that are 11.5" apart.  Note that ROS uses
+meters for distance so convert accordingly.  The sample encoder
+resolution (ticks per revolution) is from the specs for the Pololu
+131:1 motor.  Set the appropriate number for your motor/encoder
+combination.  Set the motors_reversed to True if you find your wheels
+are turning backward, otherwise set to False.
+
+The PID parameters are trickier to set.  You can start with the sample
+values but be sure to place your robot on blocks before sending it
+your first Twist command.
+
+Launching the ros\_arduino\_python Node
+---------------------------------------
+Take a look at the launch file arduino.launch in the
+ros\_arduino\_python/launch directory.  As you can see, it points to a
+config file called my\_arduino\_params.yaml.  If you named your config
+file something different, change the name in the launch file.
+
+With your Arduino connected and running the MegaRobogaiaPololu sketch,
+launch the ros\_arduino\_python node with your parameters:
+
+    $ roslaunch ros_arduino_python arduino.launch
+
+You should see something like the following output:
+
+<pre>
+process[arduino-1]: started with pid [6098]
+Connecting to Arduino on port /dev/ttyUSB0 ...
+Connected at 57600
+Arduino is ready.
+[INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyUSB0 at 57600 baud
+[INFO] [WallTime: 1355498525.966825] motor_current_right {'rate': 5, 'type': 'PololuMotorCurrent', 'pin': 1}
+[INFO]
+etc
+</pre>
+
+If you have any Ping sonar sensors on your robot and you defined them
+in your config file, they should start flashing to indicate you have
+made the connection.
+
+Viewing Sensor Data
+-------------------
+To see the aggregated sensor data, echo the sensor state topic:
+
+    $ rostopic echo /arduino/sensor_state
+
+To see the data on any particular sensor, echo its topic name:
+
+    $ rostopic echo /arduino/sensor/sensor_name
+
+For example, if you have a sensor called ir\_front\_center, you can see
+its data using:
+
+    $ rostopic echo /arduino/sensor/ir_front_center
+
+You can also graph the range data using rxplot:
+
+    $ rxplot -p 60 /arduino/sensor/ir_front_center/range
+
+
+Sending Twist Commands and Viewing Odometry Data
+------------------------------------------------
+
+Place your robot on blocks, then try publishing a Twist command:
+
+    $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{ angular: {z: 0.5} }'
+
+The wheels should turn in a direction consistent with a
+counter-clockwise rotation (right wheel forward, left wheel backward).
+If they turn in the opposite direction, set the motors_reversed
+parameter in your config file to the opposite of its current setting,
+then kill and restart the arduino.launch file.
+
+Stop the robot with the command:
+
+    $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'
+
+To view odometry data:
+
+    $ rostopic echo /odom
+
+or
+
+   $ rxplot -p 60 /odom/pose/pose/position/x:y, /odom/twist/twist/linear/x, /odom/twist/twist/angular/z
+
+ROS Services
+------------
+The ros\_arduino\_python package also defines a few ROS services as follows:
+
+**digital\_set\_direction** - set the direction of a digital pin
+
+    $ rosservice call /arduino/digital_set_direction pin direction
+
+where pin is the pin number and direction is 0 for input and 1 for output.
+
+**digital\_write** - send a LOW (0) or HIGH (1) signal to a digital pin
+
+    $ rosservice call /arduino/digital_write pin value
+
+where pin is the pin number and value is 0 for LOW and 1 for HIGH.
+
+**servo\_write** - set the position of a servo
+
+    $ rosservice call /arduino/servo_write id pos
+
+where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in radians (0 - 3.14).
+
+**servo\_read** - read the position of a servo
+
+    $ rosservice call /arduino/servo_read id
+
+where id is the index of the servo as defined in the Arduino sketch (servos.h)
+
+Using the on-board wheel encoder counters (Arduino Uno only)
+------------------------------------------------------------
+The firmware supports on-board wheel encoder counters for Arduino Uno.
+This allows connecting wheel encoders directly to the Arduino board, without the need for any additional wheel encoder counter equipment (such as a RoboGaia encoder shield).
+
+For speed, the code is directly addressing specific Atmega328p ports and interrupts, making this implementation Atmega328p (Arduino Uno) dependent. (It should be easy to adapt for other boards/AVR chips though.)
+
+To use the on-board wheel encoder counters, connect your wheel encoders to Arduino Uno as follows:
+
+    Left wheel encoder A output -- Arduino UNO pin 2
+    Left wheel encoder B output -- Arduino UNO pin 3
+
+    Right wheel encoder A output -- Arduino UNO pin A4
+    Right wheel encoder B output -- Arduino UNO pin A5
+
+Make the following changes in the ROSArduinoBridge sketch to disable the RoboGaia encoder shield, and enable the on-board one:
+
+    /* The RoboGaia encoder shield */
+    //#define ROBOGAIA
+    /* Encoders directly attached to Arduino board */
+    #define ARDUINO_ENC_COUNTER
+
+Compile the changes and upload to your controller.
+
+
+NOTES
+-----
+If you do not have the hardware required to run the base controller,
+follow the instructions below so that you can still use your
+Arduino-compatible controller to read sensors and control PWM servos.
+
+First, you need to edit the ROSArduinoBridge sketch. At the top of
+the file comment out the line:
+
+<pre>
+#define USE_BASE
+</pre>
+
+so that it looks like this:
+
+<pre>
+//#define USE_BASE
+</pre>
+
+**NOTE:** If you are using a version of the Arduino IDE previous to 1.6.6, you also need to comment out the line that looks like this in the file encoder_driver.ino:
+
+    #include "MegaEncoderCounter.h"
+
+so it looks like this:
+
+    //#include "MegaEncoderCounter.h"
+
+Compile the changes and upload to your controller.
+
+Next, edit your my\_arduino_params.yaml file and make sure the
+use\_base\_controller parameter is set to False.  That's all there is to it.

src/ros_arduino_bridge/ros_arduino_bridge/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_arduino_bridge)
+find_package(catkin REQUIRED)
+catkin_metapackage()

src/ros_arduino_bridge/ros_arduino_bridge/package.xml

@@ -0,0 +1,21 @@
+<package>
+  <name>ros_arduino_bridge</name>
+  <version>0.2.0</version>
+  <description>
+    Metapackage for ros_arduino_bridge. 
+  </description>
+  <author>Patrick Goebel</author>
+  <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/ros_arduino_bridge</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>ros_arduino_firmware</run_depend>
+  <run_depend>ros_arduino_msgs</run_depend>
+  <run_depend>ros_arduino_python</run_depend>
+
+  <export>
+   <metapackage/>
+  </export>
+</package>

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_arduino_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs message_generation)
+
+add_message_files(FILES
+                  AnalogFloat.msg
+                  Analog.msg
+                  ArduinoConstants.msg
+                  Digital.msg
+                  SensorState.msg
+                  InfraredSensors.msg
+                  UpdatePID.msg
+                 )
+
+add_service_files(FILES
+                  DigitalSetDirection.srv
+                  DigitalWrite.srv
+                  DigitalRead.srv
+                  ServoRead.srv
+                  ServoWrite.srv
+                  AnalogWrite.srv
+                  AnalogRead.srv
+                  AlarmWrite.srv
+                  LightShow.srv
+                  GripperControl.srv
+                  GripperAngle.srv
+                  GetInfraredDistance.srv
+                  DynamicPID.srv
+                 )
+
+generate_messages(
+	DEPENDENCIES
+	std_msgs
+)
+
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

src/ros_arduino_bridge/ros_arduino_msgs/msg/Analog.msg

@@ -0,0 +1,3 @@
+# Reading from a single analog IO pin.
+Header header
+uint16 value

src/ros_arduino_bridge/ros_arduino_msgs/msg/AnalogFloat.msg

@@ -0,0 +1,3 @@
+# Float message from a single analog IO pin.
+Header header
+float64 value

src/ros_arduino_bridge/ros_arduino_msgs/msg/ArduinoConstants.msg

@@ -0,0 +1,5 @@
+# Arduino-style constants
+uint8 LOW=0
+uint8 HIGH=1
+uint8 INPUT=0
+uint8 OUTPUT=1

src/ros_arduino_bridge/ros_arduino_msgs/msg/Digital.msg

@@ -0,0 +1,4 @@
+# Reading on a digital pin
+Header header
+float32 value
+

src/ros_arduino_bridge/ros_arduino_msgs/msg/InfraredSensors.msg

@@ -0,0 +1,13 @@
+# Float message from three infrared sensors IO pin.
+std_msgs/Header header
+
+uint8 INFRARED=1
+uint8 radiation_type
+
+float32 field_of_view
+float32 min_range
+float32 max_range
+
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_msgs/msg/SensorState.msg

@@ -0,0 +1,4 @@
+Header header
+
+string[] name
+float32[] value

src/ros_arduino_bridge/ros_arduino_msgs/msg/UpdatePID.msg

@@ -0,0 +1,7 @@
+# Define Motors kp,kd params
+uint8 A_kp
+uint8 A_kd
+uint8 B_kp
+uint8 B_kd
+uint8 C_kp
+uint8 C_kd

src/ros_arduino_bridge/ros_arduino_msgs/package.xml

@@ -0,0 +1,19 @@
+<package>
+  <name>ros_arduino_msgs</name>
+  <version>0.2.0</version>
+  <description>
+    ROS Arduino Messages.
+  </description>
+  <author>corvin zhang</author>
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/ros_arduino_msgs</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+</package>

src/ros_arduino_bridge/ros_arduino_msgs/srv/AlarmWrite.srv

@@ -0,0 +1 @@
+uint8 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogRead.srv

@@ -0,0 +1,3 @@
+uint8 pin
+---
+uint16 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/AnalogWrite.srv

@@ -0,0 +1,3 @@
+uint8 pin
+uint16 value
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalRead.srv

@@ -0,0 +1,3 @@
+uint8 pin
+---
+bool value

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalSetDirection.srv

@@ -0,0 +1,3 @@
+uint8 pin
+bool direction
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/DigitalWrite.srv

@@ -0,0 +1,3 @@
+uint8 pin
+bool value
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/DynamicPID.srv

@@ -0,0 +1,7 @@
+uint8 A_kp
+uint8 A_kd
+uint8 B_kp
+uint8 B_kd
+uint8 C_kp
+uint8 C_kd
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/GetInfraredDistance.srv

@@ -0,0 +1,4 @@
+---
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperAngle.srv

@@ -0,0 +1,4 @@
+int32 servoID
+int32 angle
+int32 delay
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/GripperControl.srv

@@ -0,0 +1,3 @@
+int32 servoID
+int32 value
+---

src/ros_arduino_bridge/ros_arduino_msgs/srv/LightShow.srv

@@ -0,0 +1 @@
+uint8 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoRead.srv

@@ -0,0 +1,3 @@
+uint8 id
+---
+float32 value

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoWrite.srv

@@ -0,0 +1,3 @@
+uint8 id
+float32 value
+---

src/ros_arduino_bridge/ros_arduino_python/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_arduino_python)
+
+find_package(catkin REQUIRED)
+catkin_package(DEPENDS)
+catkin_python_setup()
+
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY nodes
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

src/ros_arduino_bridge/ros_arduino_python/README.md

@@ -0,0 +1,35 @@
+# blackTornadoRobot ros&arduino通信
+
+### 单独调试arduino夹爪部分
+首先启动ros_arduino_bridge
+
+```sh
+$ roslaunch ros_arduino_python arduino.launch
+```
+
+这时候通过
+```sh
+$ rosservice list
+```
+可以看到启动的ros service列表
+```sh
+/mobilebase_arduino/alarm_write
+/mobilebase_arduino/analog_read
+/mobilebase_arduino/analog_write
+/mobilebase_arduino/digital_read
+/mobilebase_arduino/digital_set_direction
+/mobilebase_arduino/digital_write
+/mobilebase_arduino/get_loggers
+/mobilebase_arduino/gripper_control
+/mobilebase_arduino/light_show
+/mobilebase_arduino/set_logger_level
+/rosout/get_loggers
+/rosout/set_logger_level
+```
+其中服务/mobilebase_arduino/gripper_control用来夹爪舵机。
+可通过
+```sh
+$ rosservice call /mobilebase_arduino/gripper_control [operation]
+```
+控制舵机。其中操作指令1：夹爪上行；2：夹爪下行；4：张开夹爪；5：合上夹爪。为防止误操作，以及便于小键盘操作，数字键“3”无定义。
+

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -0,0 +1,85 @@
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:arduino节点运行时加载的参数,下面对参数进行解释:
+#   is_use_serial:与arduino通信是否使用串口,false的话就是IIC通信
+#   serial_port:串口通信时arduino的端口号
+#   serial_baud:串口通信时波特率
+#   i2c_smbus_num:系统管理总线号,树莓派为1
+#   isc_slave_addr:arduino的IIC设备地址
+# Author: corvin
+# History:
+#   20200330:init this file.
+#   20201119:新增pid调速走直线参数.
+
+is_use_serial: True
+
+# /dev/ttyACM# where is # is a number such as 0, 1 etc
+serial_port: /dev/ttyACM0
+serial_baud: 57600
+
+#raspberryPi:1, huawei altals 200DK: 2
+i2c_smbus_num: 1
+i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
+
+timeout: 0.5
+
+rate: 25
+
+cmd_topic: cmd_vel
+yaw_topic: yaw_data
+
+base_controller_rate: 10.0
+base_controller_timeout: 0.7
+
+# For a robot that uses base_footprint, change base_frame to base_footprint
+base_frame: base_footprint
+odom_name: odom
+
+# Robot drivetrain parameters
+wheel_diameter: 0.058
+wheel_track: 0.109     #L value
+encoder_resolution: 11 #12V DC motors
+gear_reduction: 78    #empty payload rpm 60 r/m
+linear_scale_correction: 1.00
+angular_scale_correction: 1.00
+
+# === PID parameters
+debugPID: False
+
+accel_limit: 0.05
+
+AWheel_Kp: 18
+AWheel_Kd: 15
+AWheel_Ki: 0
+AWheel_Ko: 10
+
+BWheel_Kp: 18
+BWheel_Kd: 15
+BWheel_Ki: 0
+BWheel_Ko: 10
+
+CWheel_Kp: 18
+CWheel_Kd: 15
+CWheel_Ki: 0
+CWheel_Ko: 10
+
+# Sensor definitions (case sensitive!):
+sensors: {
+  GP2Y0A41: {pin: 0, type: GP2Y0A41, rate: 5, direction: input},
+  batPercent: {pin: 0, type: BAT_PERCENT, rate: 0.01, direction: input},
+}
+
+# Yaw Service
+yaw_data_service: imu_node/GetYawData
+
+# Yaw Target and PID
+yaw_target: 0
+yaw_kp: 2.5
+yaw_ki: 0.2
+yaw_kd: 0.11
+
+# Forward Yaw Error and Rate
+correction_yaw_flag_timeout: 2
+correction_forward_err_yaw_data: 0.012
+correction_forward_rate: 1
+correction_th_threshold: 0.1
+

src/ros_arduino_bridge/ros_arduino_python/launch/arduino.launch

@@ -0,0 +1,13 @@
+<!--
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description:
+    启动底盘arduino的上位机launch文件.
+  Author: corvin
+  History:
+    20190722: init this file.
+-->
+<launch>
+   <node name="mobilebase_arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen" required="true">
+      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
+   </node>
+</launch>

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -0,0 +1,207 @@
+#!/usr/bin/env python
+#_*_ coding:utf-8 _*_
+
+"""
+  Copyright:2016-2020 www.corvin.cn ROS小课堂
+  Description: A ROS Node for the Arduino DUE microcontroller
+  Author: corvin
+  History:
+    20200412:init this file.
+"""
+import os
+import rospy
+import thread
+from ros_arduino_msgs.srv import *
+from geometry_msgs.msg import Twist
+from serial.serialutil import SerialException
+from ros_arduino_python.arduino_sensors import *
+from ros_arduino_python.arduino_driver import Arduino
+from ros_arduino_python.base_controller import BaseController
+
+
+class ArduinoROS():
+    def __init__(self):
+        rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
+
+        # Get the actual node name in case it is set in the launch file
+        self.name = rospy.get_name()
+
+        # Cleanup when termniating the node
+        rospy.on_shutdown(self.shutdown)
+
+        self.is_use_serial = rospy.get_param("~is_use_serial", True)
+
+        self.serial_port = rospy.get_param("~serial_port", "/dev/ttyACM0")
+        self.serial_baud = int(rospy.get_param("~serial_baud", 57600))
+        self.i2c_smbus_num = rospy.get_param("~i2c_smbus_num", 1)
+        self.i2c_slave_addr = rospy.get_param("~i2c_slave_addr", 8)
+        self.timeout    = rospy.get_param("~timeout", 0.7)
+        self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
+
+        # Overall loop rate: should be faster than fastest sensor rate
+        self.rate = int(rospy.get_param("~rate", 25))
+        loop_rate = rospy.Rate(self.rate)
+
+        # Initialize a Twist message
+        self.cmd_vel = Twist()
+
+        # A cmd_vel publisher so we can stop the robot when shutting down
+        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
+
+        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
+        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
+
+        # A service to turn a digital sensor on or off
+        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
+
+        # A service to read the value of a digital sensor
+        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
+
+        # A service to set pwm values for the pins
+        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
+
+        # A service to read the value of an analog sensor
+        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
+
+        # A service to set alarm sensor
+        rospy.Service('~alarm_write', AlarmWrite, self.AlarmWriteHandler)
+
+        # A service to set light show
+        rospy.Service('~light_show', LightShow, self.LightShowHandler)
+
+        # A service to position PWM servo
+        rospy.Service('~gripper_control', GripperControl, self.GripperControlHandler)
+
+        # A service to control servo angle
+        rospy.Service('~gripper_angle', GripperAngle, self.GripperAngleHandler)
+
+        # A service to get new pid params
+        rospy.Service('~dynamic_pid', DynamicPID, self.DynamicPIDHandler)
+
+        # A service to return infrared sensor distance
+        rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
+
+        # Initialize the controlller
+        self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
+
+        # Make the connection
+        self.controller.connect()
+
+        # Reserve a thread lock
+        mutex = thread.allocate_lock()
+
+        # Initialize any sensors
+        self.mySensors = list()
+        sensor_params = rospy.get_param("~sensors", dict({}))
+
+        for name, params in sensor_params.iteritems():
+            # Set the direction to input if not specified
+            try:
+                params['direction']
+            except:
+                params['direction'] = 'input'
+
+            if params['type'] == "GP2Y0A41":
+                sensor = GP2Y0A41(self.controller, name, params['pin'], params['rate'], self.base_frame)
+            elif params['type'] == 'Digital':
+                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+            elif params['type'] == 'Analog':
+                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+            elif params['type'] == 'BAT_PERCENT':
+                sensor = BatPercent(self.controller, name, params['pin'], params['rate'], self.base_frame)
+
+            try:
+                self.mySensors.append(sensor)
+                rospy.loginfo(name + " " + str(params) + " published on topic " + "/sensor/" + name)
+            except:
+                rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
+
+        # Initialize the base controller
+        self.myBaseController = BaseController(self.is_use_serial, self.controller, self.base_frame, self.name + "_base_controller")
+
+        # Start polling the sensors and base controller
+        while not rospy.is_shutdown():
+            mutex.acquire()
+            self.myBaseController.poll()
+            mutex.release()
+
+            for sensor in self.mySensors:
+                mutex.acquire()
+                sensor.poll()
+                mutex.release()
+
+            loop_rate.sleep()
+
+    def DigitalSetDirectionHandler(self, req):
+        self.controller.pin_mode(req.pin, req.direction)
+        return DigitalSetDirectionResponse()
+
+    def DigitalWriteHandler(self, req):
+        self.controller.digital_write(req.pin, req.value)
+        return DigitalWriteResponse()
+
+    def DigitalReadHandler(self, req):
+        value = self.controller.digital_read(req.pin)
+        return DigitalReadResponse(value)
+
+    def AnalogWriteHandler(self, req):
+        self.controller.analog_write(req.pin, req.value)
+        return AnalogWriteResponse()
+
+    def AnalogReadHandler(self, req):
+        value = self.controller.analog_read(req.pin)
+        return AnalogReadResponse(value)
+
+    def AlarmWriteHandler(self, req):
+        self.controller.alarm_write(req.value)
+        return AlarmWriteResponse()
+
+    def LightShowHandler(self, req):
+        self.controller.light_show(req.value)
+        return LightShowResponse()
+
+    def GripperControlHandler(self, req):
+        self.controller.gripper_control(req.servoID, req.value)
+        return GripperControlResponse()
+
+    def GripperAngleHandler(self, req):
+        self.controller.gripper_angle(req.servoID, req.angle, req.delay)
+        return GripperAngleResponse()
+
+    def GetInfraredDistanceHandler(self, req):
+        value = self.controller.detect_distance()
+        return GetInfraredDistanceResponse(value[0]/100.0, value[1]/100.0, value[2]/100.0)
+
+    def DynamicPIDHandler(self, req):
+        self.controller.update_pid(req.A_kp, req.A_kd, 0, 10,
+                                   req.B_kp, req.B_kd, 0, 10,
+                                   req.C_kp, req.C_kd, 0, 10 )
+        return DynamicPIDResponse()
+
+    # Stop the robot arduino connect
+    def shutdown(self):
+        rospy.logwarn("Shutting down Arduino Node...")
+        try:
+            rospy.logwarn("Stopping the robot move...")
+            self.cmd_vel_pub.Publish(Twist())
+        except:
+            pass
+
+        # Close the serial port or IIC
+        try:
+            self.controller.close()
+        except:
+            pass
+        finally:
+            os._exit(0)
+
+if __name__ == '__main__':
+    try:
+        myArduino = ArduinoROS()
+    except SerialException:
+        rospy.logerr("ERROR trying to open serial port.")
+        os._exit(0)
+    else:
+        rospy.logerr("Get other unknown error !")
+        os._exit(0)
+

src/ros_arduino_bridge/ros_arduino_python/package.xml

@@ -0,0 +1,22 @@
+<package>
+  <name>ros_arduino_python</name>
+  <version>0.2.0</version>
+  <description>
+    ROS Arduino Python.
+  </description>
+  <author>Patrick Goebel</author>
+  <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/ros_arduino_python</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>ros_arduino_msgs</run_depend>
+  <run_depend>python-serial</run_depend>
+</package>

src/ros_arduino_bridge/ros_arduino_python/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['ros_arduino_python'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/.gitignore

@@ -0,0 +1 @@
+*.pyc

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -0,0 +1,657 @@
+#!/usr/bin/env python
+#-*- coding:utf-8 -*-
+
+"""
+  Copyright: 2016-2020 ROS小课堂 www.corvin.cn
+  Description: A Python driver for the Arduino microcontroller.
+  Author: corvin, jally
+  History:
+    20200329:增加获取电池剩余电量百分比函数.
+    20200412:增加发布红外测距信息的服务.
+    20200423:增加获取电流传感器信息
+    20201119:修复i2c获取pid,编码器,电量问题.
+"""
+from serial.serialutil import SerialException
+import thread, smbus, rospy, time, os
+from serial import Serial
+import sys, traceback
+import numpy as np
+import roslib, rospy
+
+
+class Arduino:
+    ''' Configuration Arduino DUE Board Parameters
+    '''
+    N_ANALOG_PORTS  = 10
+    N_DIGITAL_PORTS = 54
+
+    def __init__(self, is_use_serial,serial_port="/dev/ttyACM0",baudrate=57600,i2c_smbus_num=1,i2c_slave_addr=8,timeout=0.5):
+        self.PID_RATE = 40   #Don't change this ! It is a fixed property of the Arduino PID controller.
+        self.PID_INTERVAL = 25
+
+        self.is_use_serial = is_use_serial #与下位机arduino的通信方式是否使用串口还是IIC
+        self.serial_port = serial_port
+        self.baudrate    = baudrate
+
+        self.i2c_smbus_num  = i2c_smbus_num
+        self.i2c_slave_addr = i2c_slave_addr
+
+        self.timeout = timeout
+
+        self.encoder_count    = 0
+        self.writeTimeout     = timeout
+        self.interCharTimeout = timeout/30.
+
+        # Keep things thread safe
+        self.mutex = thread.allocate_lock()
+
+        # An array to cache analog sensor readings
+        self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
+
+        # An array to cache digital sensor readings
+        self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
+
+    #与下位机arduino建立连接
+    def connect(self):
+        if self.is_use_serial:
+            self.serial_connect()
+        else:
+            self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
+            self.i2c_connect()
+
+    #使用串口连接进行通信
+    def serial_connect(self):
+        try:
+            rospy.loginfo("Connecting to Arduino on port: " + self.serial_port)
+            self.serial_port = Serial(port=self.serial_port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
+            # The next line is necessary to give the firmware time to wake up.
+            time.sleep(1)
+            test = self.get_baud()
+            if test != self.baudrate:
+                time.sleep(1)
+                test = self.get_baud()
+                if test != self.baudrate:
+                    raise SerialException
+
+            self.beep_ring(1)
+            rospy.loginfo("Arduino connected baudrate: "+str(self.baudrate))
+        except SerialException:
+            rospy.logerr("Cannot connect to Arduino !")
+            rospy.logerr(sys.exc_info())
+            rospy.logerr(traceback.print_exc(file=sys.stdout))
+            os._exit(1)
+
+    #使用IIC总线进行数据通信
+    def i2c_connect(self):
+        try:
+            rospy.loginfo("Connecting to Arduino on IIC addr: "+str(self.i2c_slave_addr))
+            test = self.i2c_get_baud()
+            if test != self.baudrate:
+                time.sleep(1)
+                test = self.i2c_get_baud()
+                if test != self.baudrate:
+                    raise Exception
+
+            self.beep_ring(2)
+            rospy.sleep(0.1)
+            self.beep_ring(3)
+            rospy.sleep(0.05)
+            self.beep_ring(2)
+            rospy.sleep(0.1)
+            self.beep_ring(3)
+            rospy.loginfo("Arduino is connected by IIC !")
+        except Exception:
+            rospy.logerr(sys.exc_info())
+            rospy.logerr(traceback.print_exc(file=sys.stdout))
+            rospy.logerr("Cannot connect to Arduino IIC slave addr !")
+            os._exit(1)
+
+    def open(self):
+        ''' Open the serial port.
+        '''
+        self.serial_port.open()
+
+    def close(self):
+        ''' Close the serial port or i2c bus connect.
+        '''
+        self.beep_ring(2)
+        rospy.sleep(0.5)
+        self.beep_ring(3)
+        if self.is_use_serial:
+            self.serial_port.close()
+        else:
+            self.i2c_bus.close()
+
+    def send(self, cmd):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        self.serial_port.write(cmd + '\r')
+
+    def recv(self, timeout=0.5):
+        timeout = min(timeout, self.timeout)
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner
+        '''
+        c = ''
+        value = ''
+        attempts = 0
+
+        while c != '\r':
+            c = self.serial_port.read(1)
+            value += c
+            attempts += 1
+            if attempts * self.interCharTimeout > timeout:
+                return None
+
+        value = value.strip('\r')
+        return value
+
+    def recv_ack(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        ack = self.recv(self.timeout)
+        return ack == 'OK'
+
+    def recv_int(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        value = self.recv(self.timeout)
+        try:
+            return int(value)
+        except:
+            return None
+
+    def recv_array(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        try:
+            values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
+            return map(float, values)
+        except:
+            return []
+
+    def execute(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
+        '''
+        self.mutex.acquire()
+
+        try:
+            self.serial_port.flushInput()
+        except:
+            pass
+
+        ntries   = 1
+        attempts = 0
+        try:
+            self.serial_port.write(cmd + '\r')
+            value = self.recv(self.timeout)
+            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
+                try:
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
+                    value = self.recv(self.timeout)
+                except:
+                    rospy.logerr("Exception executing command: " + cmd)
+                attempts += 1
+        except:
+            self.mutex.release()
+            rospy.logerr("Exception executing command: " + cmd)
+            value = None
+
+        self.mutex.release()
+        return value
+
+    def execute_array(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning an float array.
+        '''
+        self.mutex.acquire()
+        try:
+            self.serial_port.flushInput()
+        except:
+            pass
+
+        ntries   = 1
+        attempts = 0
+        try:
+            self.serial_port.write(cmd + '\r')
+            values = self.recv_array()
+            while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
+                try:
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
+                    values = self.recv_array()
+                except:
+                    rospy.logerr("Exception executing command: " + cmd)
+                attempts += 1
+        except:
+            self.mutex.release()
+            rospy.logerr("Exception executing command: " + cmd)
+            raise SerialException
+            return []
+
+        try:
+            values = map(float, values)
+        except:
+            values = []
+
+        self.mutex.release()
+        return values
+
+    def execute_ack(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
+        '''
+        self.mutex.acquire()
+
+        try:
+            self.serial_port.flushInput()
+        except:
+            pass
+
+        ntries   = 1
+        attempts = 0
+        try:
+            self.serial_port.write(cmd + '\r')
+            ack = self.recv(self.timeout)
+            while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
+                try:
+                    self.serial_port.flushInput()
+                    self.serial_port.write(cmd + '\r')
+                    ack = self.recv(self.timeout)
+                except:
+                    rospy.logerr("Exception executing command: " + cmd)
+                attempts += 1
+        except:
+            self.mutex.release()
+            rospy.logerr("execute_ack exception when executing" + cmd)
+            rospy.logerr(sys.exc_info())
+            return 0
+
+        self.mutex.release()
+        return ack == 'OK'
+
+    def update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
+                         BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,
+                         CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
+        ''' Set the PID parameters on the Arduino by serial
+        '''
+        rospy.loginfo("Updating Motors PID parameters ...")
+        cmd = 'u '+str(AWheel_Kp)+':'+str(AWheel_Kd)+':'+str(AWheel_Ki)+':'+str(AWheel_Ko)+':'+str(BWheel_Kp)+':'+str(BWheel_Kd)+':'+str(BWheel_Ki)+':'+str(BWheel_Ko)+':'+str(CWheel_Kp)+':'+str(CWheel_Kd)+':'+str(CWheel_Ki)+':'+str(CWheel_Ko)
+        self.execute_ack(cmd)
+        #rospy.loginfo("pid params now:"+cmd)
+
+    def i2c_update_pid(self, AWheel_Kp, AWheel_Kd, AWheel_Ki, AWheel_Ko,
+                         BWheel_Kp, BWheel_Kd, BWheel_Ki, BWheel_Ko,
+                         CWheel_Kp, CWheel_Kd, CWheel_Ki, CWheel_Ko ):
+        rospy.loginfo("Updating PID parameters by IIC")
+
+    def get_baud(self):
+        ''' Get the current baud rate on the serial port.
+        '''
+        try:
+            return int(self.execute('b'));
+        except:
+            return None
+
+    def i2c_get_baud(self):
+        ''' Get the current baud rate on the arduino IIC addr.
+        '''
+        try:
+            #rospy.loginfo("i2c_slave_addr:"+str(self.i2c_slave_addr))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('b'))
+            #if the program ends here, maybe I/O error, please check $ sudo i2cdetect -y -a 1
+            #if 0x08 does not exist all the time or the refresh is so slow, please reset arduino DUE
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+            time.sleep(0.1)
+            values = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 5)
+            values = map(chr, values)
+            ret = ''.join(values)
+            #print "ret:"+ret
+            return int(ret)
+        except:
+            rospy.loginfo("get baud failed !")
+            return None
+
+    def get_encoder_counts(self):
+        values = self.execute_array('e')
+        if len(values) != 3:
+            rospy.logerr("Encoder count was not 3")
+            raise SerialException
+            return None
+        else:
+            return values
+
+    def i2c_get_encoder_counts(self):
+        #print "IIC Get Encoder count !"
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('e')))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+            result_array = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            #There are result_flag valid bytes.
+            result_flag = result_array.index(120)
+            if result_flag == 0:
+                result_array = result_array[1:(len(result_array)-1)]
+                result_flag = result_array.index(120)
+            result_string=''.join([chr(ch) for ch in result_array[0:(result_flag-1)]])
+            #rospy.loginfo("Valid bytes: "+str(result_flag))
+            #extract encoder_counts by space
+            values=[int(s) for s in result_string.split(" ")]
+            if len(values) != 3:
+                rospy.logwarn("Encoder count was not 3")
+                return None
+            else:
+                #rospy.loginfo("Encoder counts now: "+ str(values))
+                return values
+        except:
+            rospy.logerr(sys.exe_info())
+            traceback.print_exc(file=sys.stdout)
+            return None
+
+    def reset_encoders(self):
+        ''' Reset the encoder counts to 0 by serial port
+        '''
+        return self.execute_ack('r')
+
+    def i2c_reset_encoders(self):
+        ''' Reset the encoder counts to 0 by IIC
+        '''
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('r'))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+            rospy.loginfo("Reset encoders sucessfully")
+        except:
+            return None
+
+    def gripper_control(self, servoID, operation):
+        ''' control gripper: 1:down 2:stop 3:up
+        '''
+        #rospy.loginfo("gripper operation ID:" + str(operation))
+        if self.is_use_serial:
+            return self.execute_ack('z %d %d' %(servoID, operation))
+        else:
+            '''control gripper up/down by IIC
+            '''
+            cmd = (' %d %d\r' %(servoID, operation))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('z'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def gripper_angle(self, servoID, angle, delay):
+        ''' control gripper angle-180 servo
+        '''
+        #rospy.loginfo("gripper operation ID:" + str(servoID) + ", angle:" + str(angle))
+        if self.is_use_serial:
+            return self.execute_ack('s %d %d %d' %(servoID, angle, delay))
+        else:
+            '''control gripper open/close by IIC
+            '''
+            cmd = (' %d %d %d\r' %(servoID, angle, delay))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('s'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def drive(self, AWheel, BWheel, CWheel):
+        ''' Speeds are given in encoder ticks per PID interval
+        '''
+        #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel) + ",C:" + str(CWheel))
+        return self.execute_ack('m %d %d %d' %(AWheel, BWheel, CWheel))
+
+    def i2c_drive(self, AWheel, BWheel, CWheel):
+        ''' Speeds are given in encoder ticks per PID interval
+        '''
+        #rospy.loginfo("drive() A:" + str(AWheel) + ",B:" + str(BWheel) + ",C:" + str(CWheel))
+        cmd = (' %d %d %d\r' %(AWheel, BWheel, CWheel))
+        try:
+            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
+        except:
+            return None
+
+    #使用串口发送停止电机转动命令
+    def stop(self):
+        ''' Stop all three motors.
+        '''
+        self.drive(0, 0, 0)
+
+    def i2c_stop(self):
+        cmd = (' 0 0 0\r')
+        try:
+            self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('m'), [ord(c) for c in cmd])
+        except:
+            return None
+
+    def analog_read(self, pin):
+        if self.is_use_serial:
+            return self.execute('a %d' %pin)
+        else:
+            cmd = (' %d\r' %(pin))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('a'), [ord(c) for c in cmd])
+                pin = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 3)
+                pin = map(chr, pin)
+                ret = ''.join(pin)
+                #print "ret:"+ret
+                return int(ret)
+            except:
+                return None
+
+    def analog_write(self, pin, value):
+        if self.is_use_serial:
+            return self.execute_ack('x %d %d' %(pin, value))
+        else:
+            cmd = (' %d %d\r' %(pin, value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('x'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def digital_read(self, pin):
+        if self.is_use_serial:
+            return self.execute('d %d' %pin)
+        else:
+            cmd = (' %d\r' %(pin))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('d'), [ord(c) for c in cmd])
+                pin = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 2)
+                pin = map(chr, pin)
+                #print pin
+                ret = ''.join(pin)
+                #print "ret:"+ret
+                return int(ret)
+            except:
+                return None
+
+    def digital_write(self, pin, value):
+        if self.is_use_serial:
+            return self.execute_ack('w %d %d' %(pin, value))
+        else:
+            cmd = (' %d %d\r' %(pin, value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('w'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def pin_mode(self, pin, mode):
+        if self.is_use_serial:
+            return self.execute_ack('c %d %d' %(pin, mode))
+        else:
+            cmd = (' %d %d\r' %(pin, mode))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('c'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def beep_ring(self, value):
+        if self.is_use_serial:
+            return self.execute_ack('p %d' %value)
+        else:
+            cmd = (' %d\r' %(value))
+            try:
+                self.i2c_bus.write_i2c_block_data(self.i2c_slave_addr, ord('p'), [ord(c) for c in cmd])
+            except:
+                return None
+
+    def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
+        if self.is_use_serial:
+            return self.execute_array('h')
+        else:
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('j')))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+                result_array2 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+                #There are result_flag valid bytes.
+                result_flag2 = result_array2.index(120)
+                #rospy.loginfo("Valid bytes: "+str(result_flag2))
+                #extract result_flag bytes as strings
+                result_string2=''.join([chr(ch) for ch in result_array2[0:(result_flag2-1)]])
+                #extract infrared_counts by space
+                values=[float(s) for s in result_string2.split(" ")]
+                #print values
+                if len(values) != 3:
+                    rospy.logwarn("InfraredEncoder count was not 3")
+                    return None
+                else:
+                    #rospy.loginfo("Infrared counts now: "+ str(values))
+                    return values
+            except:
+                return None
+
+    def getBatPercent(self):
+        '''get the remaining power percentage
+        '''
+        if self.is_use_serial:
+            percent = self.execute('g')
+            #rospy.loginfo("Bat percent:" + str(percent))
+            return percent
+        else:
+            try:
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('g'))
+                self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+                percent = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0, 3)
+                if percent[0] == 120:
+                    percent.remove(percent[0])
+                percent = map(chr, percent)
+                ret = ''.join(percent)
+                #print "ret:"+ret
+                return int(ret)
+            except:
+                return None
+
+    def get_pidin(self):
+        values = self.execute_array('i')
+        if len(values) != 3:
+            rospy.logerr("pidin was not 3")
+            raise SerialException
+            return None
+        else:
+            return values
+
+    def i2c_get_pidin(self):
+        # rospy.loginfo("IIC Get Pidin !")
+
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('i')))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+            result_array3 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            #print(result_array3)
+            #There are result_flag valid bytes.
+            result_flag3 = result_array3.index(120)
+            #print result_flag3
+            #rospy.loginfo("Valid bytes: "+str(result_flag))
+            #extract result_flag bytes as strings
+            if result_flag3 == 0:
+                result_array3 = result_array3[1:(len(result_array3)-1)]
+                result_flag3 = result_array3.index(120)
+                result_string3=''.join([chr(ch) for ch in result_array3[0:(result_flag3-1)]])
+            else:
+                result_string3=''.join([chr(ch) for ch in result_array3[0:(result_flag3-1)]])
+            #print result_string3
+            #extract Pidin_counts by space
+            values=[int(s) for s in result_string3.split(" ")]
+            if len(values) != 3:
+                rospy.logwarn("Pidin was not 3")
+                return None
+            else:
+                #rospy.loginfo("Pidin now: "+ str(values))
+                return values
+        except:
+            rospy.logerr(sys.exe_info())
+            traceback.print_exc(file=sys.stdout)
+            return None
+
+    def get_pidout(self):
+        values = self.execute_array('o')
+        if len(values) != 3:
+            rospy.logerr("pidout was not 3")
+            raise SerialException
+            return None
+        else:
+            return values
+
+    def i2c_get_pidout(self):
+        # rospy.loginfo("IIC Get Pidout !")
+
+        try:
+            self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('o')))
+            self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
+
+            result_array4 = self.i2c_bus.read_i2c_block_data(self.i2c_slave_addr, 0x08)
+            # print(result_array4)
+            #There are result_flag valid bytes.
+            result_flag4 = result_array4.index(120)
+            #print result_flag4
+            #rospy.loginfo("Valid bytes: "+str(result_flag))
+            #extract result_flag bytes as strings
+            result_string4=''.join([chr(ch) for ch in result_array4[0:(result_flag4-1)]])
+            #print result_string4
+            #extract Pidout_counts by space
+            values=[int(s) for s in result_string4.split(" ")]
+            if len(values) != 3:
+                rospy.logwarn("Pidout was not 3")
+                return None
+            else:
+                #rospy.loginfo("Pidout now: "+ str(values))
+                return values
+        except:
+            rospy.logerr(sys.exe_info())
+            traceback.print_exc(file=sys.stdout)
+            return None
+
+
+""" Basic test for connectivity by serial port or IIC bus"""
+if __name__ == "__main__":
+    is_use_serial = True
+    portName = "/dev/ttyACM0"
+    baudRate = 57600
+
+    iic_smbus = 1
+    iic_num = 8
+
+    myArduino = Arduino(is_use_serial, serial_port=portName, baudrate=baudRate, i2c_smbus_num=iic_smbus, i2c_slave_addr=iic_num, timeout=0.5)
+    myArduino.connect()
+
+    rospy.loginfo("Sleeping for 1 second...")
+    time.sleep(1)
+
+    rospy.loginfo("Test Beep ring 3 times...")
+    for i in range(3):
+        myArduino.beep_ring(1)
+        time.sleep(2.0)
+
+    rospy.loginfo("Did you heard the beep ring ?")
+    rospy.loginfo("Now print infrared sensors value:")
+    values = myArduino.detect_distance()
+    distances = np.array([values[0], values[1], values[2]])
+    rospy.loginf(distances/100.0)
+
+    myArduino.stop()
+    myArduino.close()
+    rospy.logwarn("Shutting down Arduino !")
+

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -0,0 +1,254 @@
+#!/usr/bin/env python
+#_*_ coding:utf-8 _*_
+
+"""
+    Sensor class for the arudino_python package
+    History:
+        20200330:增加获取电池剩余电量函数.
+        20200423:增加获取电流函数
+"""
+import roslib; roslib.load_manifest('ros_arduino_python')
+import rospy
+import numpy as np
+from decimal import Decimal
+from sensor_msgs.msg import Range
+from ros_arduino_msgs.msg import *
+
+
+LOW = 0
+HIGH = 1
+
+INPUT = 0
+OUTPUT = 1
+
+class MessageType:
+    ANALOG   = 0
+    DIGITAL  = 1
+    INFRARED = 2
+    RANGE    = 3
+    FLOAT    = 4
+    INT      = 5
+    BOOL     = 6
+
+class Sensor(object):
+    def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
+        self.controller = controller
+        self.name = name
+        self.pin = pin
+        self.rate = rate
+        self.direction = direction
+
+        self.frame_id = frame_id
+        self.value = None
+
+        self.t_delta = rospy.Duration(1.0 / self.rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+
+
+    def poll(self):
+        now = rospy.Time.now()
+        if now > self.t_next:
+            if self.direction == "input":
+                try:
+                    self.value = self.read_value()
+                    #rospy.loginfo("read value: "+str(self.value))
+                except:
+                    try:
+                        # try again
+                        rospy.logwarn("Failed to read sensor values, try again. ")
+                        self.ack = self.write_value()
+                    except:
+                        rospy.logerr("Sensor read value error !")
+                        return
+            else:
+                try:
+                    self.ack = self.write_value()
+                except:
+                    rospy.logerr("Sensor write value error !")
+                    return
+
+            # For range sensors, assign the value to the range message field
+            if self.message_type == MessageType.RANGE:
+                self.msg.range = self.value
+            elif self.message_type == MessageType.INFRARED:
+                self.msg.front_dist = self.value[0]
+                self.msg.left_dist  = self.value[1]
+                self.msg.right_dist = self.value[2]
+            else:
+                self.msg.value = self.value
+
+            # Add a timestamp and publish the message
+            self.msg.header.stamp = rospy.Time.now()
+            self.pub.publish(self.msg)
+
+            self.t_next = now + self.t_delta
+
+class AnalogSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(AnalogSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.ANALOG
+        self.msg = Analog()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
+        self.value = LOW
+
+    def read_value(self):
+        return self.controller.analog_read(self.pin)
+
+    def write_value(self, value):
+        return self.controller.analog_write(self.pin, value)
+
+class AnalogFloatSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(AnalogFloatSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.ANALOG
+
+        self.msg = AnalogFloat()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
+
+        if self.direction == "output":
+            self.controller.pin_mode(self.pin, OUTPUT)
+        else:
+            self.controller.pin_mode(self.pin, INPUT)
+
+        self.value = LOW
+
+    def read_value(self):
+        return self.controller.analog_read(self.pin)
+
+    def write_value(self, value):
+        return self.controller.analog_write(self.pin, value)
+
+class DigitalSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(DigitalSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.DIGITAL
+        self.msg = Digital()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
+        self.value = LOW
+
+    def read_value(self):
+        return self.controller.digital_read(self.pin)
+
+    def write_value(self):
+        # Alternate HIGH/LOW when writing at a fixed rate
+        self.value = not self.value
+        return self.controller.digital_write(self.pin, self.value)
+
+class RangeSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(RangeSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.RANGE
+        self.msg = Range()
+        self.msg.header.frame_id = self.frame_id
+        self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
+
+    def read_value(self):
+        self.msg.header.stamp = rospy.Time.now()
+
+class MyRangeSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(MyRangeSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.INFRARED
+        self.msg = InfraredSensors()
+        self.msg.header.frame_id = self.frame_id
+
+        self.pub = rospy.Publisher("~sensor/" + self.name, InfraredSensors, queue_size=2)
+
+    def read_value(self):
+        self.msg.header.stamp = rospy.Time.now()
+
+class SonarSensor(RangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(SonarSensor, self).__init__(*args, **kwargs)
+        self.msg.radiation_type = Range.ULTRASOUND
+
+class IRSensor(RangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(IRSensor, self).__init__(*args, **kwargs)
+        self.msg.radiation_type = Range.INFRARED
+
+class MyIRSensor(MyRangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(MyIRSensor, self).__init__(*args, **kwargs)
+        self.msg.radiation_type = Range.INFRARED
+
+class GP2Y0A41(MyIRSensor):
+    def __init__(self, *args, **kwargs):
+        super(GP2Y0A41, self).__init__(*args, **kwargs)
+
+        self.msg.field_of_view = 0.01
+        self.msg.min_range = 0.04
+        self.msg.max_range = 0.30
+
+    def read_value(self):
+        values = self.controller.detect_distance()
+        distances = np.array([values[0], values[1], values[2]])
+        return distances/100.0
+
+class IR2Y0A02(IRSensor):
+    def __init__(self, *args, **kwargs):
+        super(IR2Y0A02, self).__init__(*args, **kwargs)
+
+        self.msg.field_of_view = 0.001
+        self.msg.min_range = 0.200
+        self.msg.max_range = 1.500
+
+    def read_value(self):
+        value = self.controller.analog_read(self.pin)
+
+        try:
+            volts = value*0.0048828125;
+            distance = 65*pow(volts, -1.10)
+        except:
+            return self.msg.min_range
+
+        # Convert to meters
+        distance /= 100.000
+        dist = round(float(distance), 3)
+
+        # If we get a spurious reading, set it to the max_range
+        if dist > self.msg.max_range: dist = self.msg.max_range
+        if dist < self.msg.min_range: dist = self.msg.min_range
+
+        return dist
+
+class MotorTotalCurrent(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(MotorTotalCurrent, self).__init__(*args, **kwargs)
+
+    def read_value(self):
+        midVal = mylist[15]
+        result = (midVal/1024.0*4523.00 - 4523.00/2)/100
+        return Decimal(result).quantize(Decimal('0.00'))
+
+class BatPercent(DigitalSensor):
+    def __init__(self, *args, **kwargs):
+        super(BatPercent, self).__init__(*args, **kwargs)
+
+    def read_value(self):
+        percent = self.controller.getBatPercent()
+        return int(percent)
+
+class CurrentValue(DigitalSensor):
+    def __init__(self, *args, **kwargs):
+        super(CurrentValue, self).__init__(*args, **kwargs)
+
+    def read_value(self):
+        currentvalue = self.controller.getCurrentValue()
+        #rospy.loginfo("read currentvalue: "+currentvalue)
+        return float(currentvalue)
+
+
+if __name__ == '__main__':
+    rospy.loginf("arduino sensor main function ...")

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -0,0 +1,520 @@
+#!/usr/bin/env python
+#-*- coding:utf-8 -*-
+
+"""
+  Copyright: 2016-2020 www.corvin.cn ROS小课堂
+  Description:  A base controller class for the Arduino DUE microcontroller
+  Author: corvin
+  History:
+    20200704:当超过给下位机发送控制移动命令时间后,不需要再发送pwm控制0的命令.
+        因为在arduino代码会根据超时自己会执行pwm清零停止电机移动的命令.
+    20201119:新增pid调速走直线,但横向位移仍未解决.
+"""
+import os
+import rospy
+import time
+import roslib; roslib.load_manifest('ros_arduino_python')
+import dynamic_reconfigure.client
+
+from math import sin, cos, pi, sqrt
+import numpy as np
+from geometry_msgs.msg import Quaternion, Twist
+from nav_msgs.msg import Odometry
+from tf.broadcaster import TransformBroadcaster
+from std_msgs.msg import Int32
+from std_msgs.msg import Float32
+from serial_imu_hat_6dof.srv import GetYawData
+
+class SimplePID:
+	'''very simple discrete PID controller'''
+	def __init__(self, target, P, I, D):
+		'''Create a discrete PID controller
+		each of the parameters may be a vector if they have the same length
+
+		Args:
+		target (double) -- the target value(s)
+		P, I, D (double)-- the PID parameter
+
+		'''
+
+		# check if parameter shapes are compatabile.
+		self.Kp		=np.array(P)
+		self.Ki		=np.array(I)
+		self.Kd		=np.array(D)
+		self.setPoint   =np.array(target)
+
+		self.last_error=0
+		self.integrator = 0
+		self.integrator_max = float('inf')
+		self.timeOfLastCall = None
+
+
+	def update(self, current_value):
+		'''Updates the PID controller.
+
+		Args:
+			current_value (double): vector/number of same legth as the target given in the constructor
+
+		Returns:
+			controll signal (double): vector of same length as the target
+
+		'''
+		current_value=np.array(current_value)
+		if(self.timeOfLastCall is None):
+			# the PID was called for the first time. we don't know the deltaT yet
+			# no controll signal is applied
+			self.timeOfLastCall = time.clock()
+			return np.zeros(np.size(current_value))
+
+
+		error = self.setPoint - current_value
+		P =  error
+
+		currentTime = time.clock()
+		deltaT      = (currentTime-self.timeOfLastCall)
+
+		# integral of the error is current error * time since last update
+		self.integrator = self.integrator + (error*deltaT)
+		I = self.integrator
+
+		# derivative is difference in error / time since last update
+		D = (error-self.last_error)/deltaT
+
+		self.last_error = error
+		self.timeOfLastCall = currentTime
+
+		# return controll signal
+		return self.Kp*P + self.Ki*I + self.Kd*D
+
+""" Class to receive Twist commands and publish wheel Odometry data """
+class BaseController:
+
+    def __init__(self, is_use_serial, arduino, base_frame, name="base_controllers_node"):
+        self.use_serial = is_use_serial
+        self.arduino    = arduino
+        self.name       = name
+        self.base_frame = base_frame
+        self.odom_name  = rospy.get_param("~odom_name", "odom")
+        self.cmd_topic  = rospy.get_param("~cmd_topic", "cmd_vel")
+        self.yaw_topic_name = rospy.get_param('~yaw_topic', 'yaw_topic')
+        self.rate       = float(rospy.get_param("~base_controller_rate", 20))
+        self.timeout    = rospy.get_param("~base_controller_timeout", 0.7)
+
+        self.yaw_data_service = rospy.get_param("~yaw_data_service","imu_node/GetYawData")
+        self.yaw_target = rospy.get_param("~yaw_target",0)
+        self.yaw_kp = rospy.get_param("~yaw_kp",1)
+        self.yaw_ki = rospy.get_param("~yaw_ki",1)
+        self.yaw_kd = rospy.get_param("~yaw_kd",1)
+        self.correction_forward_err_yaw_data = rospy.get_param("~correction_forward_err_yaw_data",0.017)
+        self.correction_forward_rate = rospy.get_param("~correction_forward_rate",1)
+        self.correction_yaw_flag_timeout = rospy.get_param("~correction_yaw_flag_timeout",2)
+        self.correction_th_threshold = rospy.get_param("~correction_th_threshold",0.1)
+        self.pid_controller = SimplePID(self.yaw_target, self.yaw_kp, self.yaw_ki, self.yaw_kd)
+        self.correction_yaw_flag = 0
+        self.last_yaw_data = 0
+        self.now_yaw_data = 0
+        self.initial_odom_yaw_data = self.get_yaw()
+        self.yaw_topic_data = 0
+
+        self.debugPID   = rospy.get_param('~debugPID', False)
+
+        pid_params = dict()
+        pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.058)
+        pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
+        pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", 11)
+        pid_params['gear_reduction']     = rospy.get_param("~gear_reduction", 46)
+        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 15)
+        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
+        pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
+        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)
+
+        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 15)
+        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
+        pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
+        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)
+
+        pid_params['CWheel_Kp'] = rospy.get_param("~CWheel_Kp", 15)
+        pid_params['CWheel_Kd'] = rospy.get_param("~CWheel_Kd", 15)
+        pid_params['CWheel_Ki'] = rospy.get_param("~CWheel_Ki", 0)
+        pid_params['CWheel_Ko'] = rospy.get_param("~CWheel_Ko", 50)
+
+        self.accel_limit  = rospy.get_param('~accel_limit', 0.05)
+        self.linear_scale_correction  = rospy.get_param("~linear_scale_correction",  1.0)
+        self.angular_scale_correction = rospy.get_param("~angular_scale_correction", 1.0)
+
+        #rospy.loginfo("pid_params_dict: "+pid_params)
+
+        # Set up PID parameters and check for missing values
+        self.setup_pid(pid_params)
+
+        # How many encoder ticks are there per meter?
+        self.ticks_per_meter = self.encoder_resolution*self.gear_reduction*2/(self.wheel_diameter*pi)
+        self.ticks_per_meter = self.ticks_per_meter/self.linear_scale_correction
+
+        # What is the maximum acceleration we will tolerate when changing wheel speeds?
+        self.max_accel = self.accel_limit*self.ticks_per_meter/self.rate
+
+        # Track how often we get a bad encoder count (if any)
+        self.bad_encoder_count = 0
+
+        now = rospy.Time.now()
+        self.then    = now  # time for determining dx/dy
+        self.t_delta = rospy.Duration(1.0/self.rate)
+        self.t_next  = now + self.t_delta
+
+        # Internal data
+        self.enc_A = 0  # encoder readings
+        self.enc_B = 0
+        self.enc_C = 0
+
+        self.pose_x  = 0  # position in xy plane
+        self.pose_y  = 0
+        self.th = 0  # rotation in radians
+
+        #存储三个车轮的当前转动速度
+        self.v_A = 0
+        self.v_B = 0
+        self.v_C = 0
+
+        #存储需要控制三个车轮在PID周期内控制电机转动的脉冲数
+        self.v_des_AWheel = 0  # cmd_vel setpoint
+        self.v_des_BWheel = 0
+        self.v_des_CWheel = 0
+
+        self.last_cmd_time = now
+
+        # Subscriptions main comtrol board to send control cmd
+        rospy.Subscriber(self.cmd_topic, Twist, self.cmdVelCallback)
+        rospy.Subscriber(self.yaw_topic_name,Float32,self.yawCallback)
+
+        # Clear any old odometry info
+        if self.use_serial:
+            self.arduino.reset_encoders()
+        else:
+            self.arduino.i2c_reset_encoders()
+
+        # Set up the wheel odometry broadcaster
+        self.odomPub = rospy.Publisher(self.odom_name, Odometry, queue_size=5)
+        self.odomBroadcaster = TransformBroadcaster()
+
+        #corvin add for rqt_plot debug pid
+        if self.debugPID:
+            self.AEncoderPub = rospy.Publisher('Aencoder', Int32, queue_size=5)
+            self.BEncoderPub = rospy.Publisher('Bencoder', Int32, queue_size=5)
+            self.CEncoderPub = rospy.Publisher('Cencoder', Int32, queue_size=5)
+            self.APidoutPub  = rospy.Publisher('Apidout',  Int32, queue_size=5)
+            self.BPidoutPub  = rospy.Publisher('Bpidout',  Int32, queue_size=5)
+            self.CPidoutPub  = rospy.Publisher('Cpidout',  Int32, queue_size=5)
+            self.AVelPub     = rospy.Publisher('Avel',     Int32, queue_size=5)
+            self.BVelPub     = rospy.Publisher('Bvel',     Int32, queue_size=5)
+            self.CVelPub     = rospy.Publisher('Cvel',     Int32, queue_size=5)
+
+        rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m with " + str(self.encoder_resolution) + " ticks per rev")
+        rospy.loginfo("Publishing odometry at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
+
+    def setup_pid(self, pid_params):
+        # Check to see if any PID parameters are missing
+        missing_params = False
+        for param in pid_params:
+            if pid_params[param] == "":
+                print("*** PID Parameter " + param + " is missing. ***")
+                missing_params = True
+
+        if missing_params:
+            os._exit(1)
+
+        self.encoder_resolution = pid_params['encoder_resolution']
+        self.gear_reduction     = pid_params['gear_reduction']
+        self.wheel_diameter = pid_params['wheel_diameter']
+        self.wheel_track    = pid_params['wheel_track']
+        self.wheel_track    = self.wheel_track/self.angular_scale_correction
+
+        self.AWheel_Kp = pid_params['AWheel_Kp']
+        self.AWheel_Kd = pid_params['AWheel_Kd']
+        self.AWheel_Ki = pid_params['AWheel_Ki']
+        self.AWheel_Ko = pid_params['AWheel_Ko']
+
+        self.BWheel_Kp = pid_params['BWheel_Kp']
+        self.BWheel_Kd = pid_params['BWheel_Kd']
+        self.BWheel_Ki = pid_params['BWheel_Ki']
+        self.BWheel_Ko = pid_params['BWheel_Ko']
+
+        self.CWheel_Kp = pid_params['CWheel_Kp']
+        self.CWheel_Kd = pid_params['CWheel_Kd']
+        self.CWheel_Ki = pid_params['CWheel_Ki']
+        self.CWheel_Ko = pid_params['CWheel_Ko']
+
+        if self.use_serial:
+            self.arduino.update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
+                                    self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
+                                    self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,)
+        else:
+            self.arduino.i2c_update_pid(self.AWheel_Kp, self.AWheel_Kd, self.AWheel_Ki, self.AWheel_Ko,
+                                        self.BWheel_Kp, self.BWheel_Kd, self.BWheel_Ki, self.BWheel_Ko,
+                                        self.CWheel_Kp, self.CWheel_Kd, self.CWheel_Ki, self.CWheel_Ko,)
+
+    # main loop, always run
+    def poll(self):
+        self.send_debug_pid()
+
+        time_now = rospy.Time.now() #获取当前系统时间
+        if time_now > self.t_next:
+            # 读取三个电机反馈的编码器计数
+            try:
+                if self.use_serial:
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()
+                    #rospy.loginfo("get new encoder: "+str(aWheel_enc)+ " "+str(bWheel_enc)+" "+str(cWheel_enc))
+                else:
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
+                    #rospy.loginfo("get encoder counts: "++str(aWheel_enc)+ " "+str(bWheel_enc)+" "+str(cWheel_enc))
+            except:
+                try:
+                    # try again
+                    #rospy.logwarn("Failed to get encoder counts, try again. ")
+                    aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.i2c_get_encoder_counts()
+                except:
+                    self.bad_encoder_count += 1
+                    rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
+                    return
+
+            #当读取到最新的编码器计数为0,说明下位机已经将计数清零，那上位机保存的上一次编码器计数也得清零
+            if aWheel_enc == 0:
+                self.enc_A = 0
+
+            if bWheel_enc == 0:
+                self.enc_B = 0
+
+            if cWheel_enc == 0:
+                self.enc_C = 0
+
+            #根据编码器两次反馈的差值来计算车轮移动的距离
+            dist_A = (aWheel_enc - self.enc_A)/self.ticks_per_meter
+            dist_B = (bWheel_enc - self.enc_B)/self.ticks_per_meter
+            dist_C = (cWheel_enc - self.enc_C)/self.ticks_per_meter
+            #rospy.loginfo("get delta dist:"+str(dist_A)+" "+str(dist_B)+" "+str(dist_C))
+
+            #将当前读取到的编码器计数保存,为了下次计算新脉冲数准备
+            self.enc_A = aWheel_enc
+            self.enc_B = bWheel_enc
+            self.enc_C = cWheel_enc
+            #rospy.loginfo("get save encoder:"+str(self.enc_A)+" "+str(self.enc_B)+" "+str(self.enc_C))
+
+            #计算两次获取编码器反馈的时间差
+            delta_time = time_now - self.then
+            delta_time = delta_time.to_sec()
+
+            #计算三个车轮的转动线速度
+            va = dist_A/delta_time
+            vb = dist_B/delta_time
+            vc = dist_C/delta_time
+            self.then = time_now
+            #rospy.loginfo("get vel:"+str(va)+ " "+str(vb)+" "+str(vc)+" delta_time:"+str(delta_time))
+
+            #正向运动学模型:根据三个车轮的速度合成小车整体的移动速度
+            vx  = sqrt(3)*(va - vb)/3.0
+            vy  = (va + vb - 2*vc)/3.0
+            vth = (va + vb + vc)/(3*self.wheel_track)
+
+            #计算移动的距离
+            delta_x  = (vx*cos(self.th) - vy*sin(self.th))*delta_time
+            delta_y  = (vx*sin(self.th) + vy*cos(self.th))*delta_time
+            delta_th = vth*delta_time
+            #rospy.loginfo("get delta pose:"+str(delta_x)+ " "+str(delta_y)+" "+str(delta_th))
+
+            #将移动的距离累加运算
+            self.pose_x += delta_x
+            self.pose_y += delta_y
+            self.th += delta_th
+
+            self.now_odom_yaw_data = self.yaw_topic_data
+            self.th = self.now_odom_yaw_data - self.initial_odom_yaw_data
+
+            quaternion   = Quaternion()
+            quaternion.x = 0.0
+            quaternion.y = 0.0
+            quaternion.z = sin(self.th/2.0)
+            quaternion.w = cos(self.th/2.0)
+
+##########################################################
+            # create the odometry transform frame broadcaster.
+            """
+            self.odomBroadcaster.sendTransform(
+                (self.pose_x, self.pose_y, 0),
+                (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
+                rospy.Time.now(),
+                self.base_frame,
+                self.odom_name
+            )
+            """
+##########################################################
+            odom = Odometry()
+            odom.header.frame_id = self.odom_name
+            odom.child_frame_id  = self.base_frame
+            odom.header.stamp    = time_now
+            odom.pose.pose.position.x  = self.pose_x
+            odom.pose.pose.position.y  = self.pose_y
+            odom.pose.pose.position.z  = 0
+            odom.pose.pose.orientation = quaternion
+            odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
+                                    0, 1e-3, 1e-9, 0, 0, 0,
+                                    0, 0, 1e6, 0, 0, 0,
+                                    0, 0, 0, 1e6, 0, 0,
+                                    0, 0, 0, 0, 1e6, 0,
+                                    0, 0, 0, 0, 0, 1e-9]
+            odom.twist.twist.linear.x  = vx
+            odom.twist.twist.linear.y  = vy
+            odom.twist.twist.angular.z = vth
+            odom.twist.covariance = [1e-9, 0, 0, 0, 0, 0,
+                                    0, 1e-3, 1e-9, 0, 0, 0,
+                                    0, 0, 1e6, 0, 0, 0,
+                                    0, 0, 0, 1e6, 0, 0,
+                                    0, 0, 0, 0, 1e6, 0,
+                                    0, 0, 0, 0, 0, 1e-9]
+            self.odomPub.publish(odom)
+
+            # send motor cmd
+            if time_now > (self.last_cmd_time + rospy.Duration(self.timeout)):
+                self.stop()
+                self.correction_yaw_flag = 0
+                self.reset_pid_controller()
+            else:
+                self.send_motor_speed()
+
+            # set next compare time
+            self.t_next = time_now + self.t_delta
+
+
+    # debug motor pid parameter
+    def send_debug_pid(self):
+        if self.debugPID:
+            try:
+                if self.use_serial:
+                    A_pidin, B_pidin, C_pidin = self.arduino.get_pidin()
+                else:
+                    A_pidin, B_pidin, C_pidin = self.arduino.i2c_get_pidin()
+                self.AEncoderPub.publish(A_pidin)
+                self.BEncoderPub.publish(B_pidin)
+                self.CEncoderPub.publish(C_pidin)
+            except:
+                rospy.logerr("getpidin exception count:")
+                return
+
+            try:
+                if self.use_serial:
+                    A_pidout, B_pidout, C_pidout = self.arduino.get_pidout()
+                else:
+                    A_pidout, B_pidout, C_pidout = self.arduino.i2c_get_pidout()
+                self.APidoutPub.publish(A_pidout)
+                self.BPidoutPub.publish(B_pidout)
+                self.CPidoutPub.publish(C_pidout)
+            except:
+                rospy.logerr("getpidout exception count")
+                return
+
+    # normalize motor dest encode value and send
+    def send_motor_speed(self):
+        if self.v_A < self.v_des_AWheel:
+          self.v_A += self.max_accel
+          if self.v_A > self.v_des_AWheel:
+              self.v_A = self.v_des_AWheel
+        else:
+          self.v_A -= self.max_accel
+          if self.v_A < self.v_des_AWheel:
+              self.v_A = self.v_des_AWheel
+
+        if self.v_B < self.v_des_BWheel:
+          self.v_B += self.max_accel
+          if self.v_B > self.v_des_BWheel:
+              self.v_B = self.v_des_BWheel
+        else:
+          self.v_B -= self.max_accel
+          if self.v_B < self.v_des_BWheel:
+              self.v_B = self.v_des_BWheel
+
+        if self.v_C < self.v_des_CWheel:
+          self.v_C += self.max_accel
+          if self.v_C > self.v_des_CWheel:
+              self.v_C = self.v_des_CWheel
+        else:
+          self.v_C -= self.max_accel
+          if self.v_C < self.v_des_CWheel:
+              self.v_C = self.v_des_CWheel
+
+        # send to arduino to drive motor
+        if self.use_serial:
+            self.arduino.drive(self.v_A, self.v_B, self.v_C)
+        else:
+            self.arduino.i2c_drive(self.v_A, self.v_B, self.v_C)
+
+        if self.debugPID:
+            self.AVelPub.publish(self.v_A)
+            self.BVelPub.publish(self.v_B)
+            self.CVelPub.publish(self.v_C)
+
+
+    # stop move mobileBase
+    def stop(self):
+        self.v_A = 0
+        self.v_B = 0
+        self.v_C = 0
+        self.v_des_AWheel = 0
+        self.v_des_BWheel = 0
+        self.v_des_CWheel = 0
+
+    def get_yaw(self):
+        rospy.wait_for_service(self.yaw_data_service)
+        try:
+            yaw_data = rospy.ServiceProxy(self.yaw_data_service, GetYawData)
+            resp = yaw_data()
+            return resp.yaw
+        except rospy.ServiceException, e:
+            print "Service call failed: %s"%e
+
+    def reset_pid_controller(self):
+        self.pid_controller.integrator = 0
+        self.pid_controller.last_error = 0
+        self.pid_controller.timeOfLastCall = None
+
+    def cmdVelCallback(self, req):
+
+        # Handle velocity-based movement requests
+        self.last_cmd_time = rospy.Time.now()
+
+        x  = req.linear.x  # m/s
+        y  = req.linear.y  # m/s
+        th = req.angular.z # rad/s
+
+        if x != 0 and y == 0 and th == 0:
+            if self.correction_yaw_flag < self.correction_yaw_flag_timeout:
+                self.stop()
+                self.last_yaw_data = self.yaw_topic_data
+                self.correction_yaw_flag += 1
+                return
+            else:
+                self.now_yaw_data = self.yaw_topic_data
+                err_yaw_data = self.now_yaw_data - self.last_yaw_data
+                if abs(err_yaw_data) > self.correction_forward_err_yaw_data:
+                    th = self.pid_controller.update(err_yaw_data)
+                    if th > self.correction_th_threshold:
+                        th = self.correction_th_threshold
+                    if th < -self.correction_th_threshold:
+                        th = -self.correction_th_threshold
+                    #print("err_yaw_data is ",err_yaw_data,"th is ",th)
+        else:
+            self.correction_yaw_flag = 0
+            self.reset_pid_controller()
+
+        #Inverse Kinematic
+        tmpX = sqrt(3)/2.0
+        tmpY = 0.5  # 1/2
+        vA = ( tmpX*x + tmpY*y + self.wheel_track*th)
+        vB = (-tmpX*x + tmpY*y + self.wheel_track*th)
+        vC = (              -y + self.wheel_track*th)
+
+        self.v_des_AWheel = int(vA * self.ticks_per_meter / self.arduino.PID_RATE)
+        self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)
+        self.v_des_CWheel = int(vC * self.ticks_per_meter / self.arduino.PID_RATE)
+        #rospy.loginfo("cmdCallback(): A v_des:"+str(self.v_des_AWheel)+",B v_des:"+str(self.v_des_BWheel)+",C v_des:"+str(self.v_des_CWheel))
+
+    def yawCallback(self,req):
+        self.yaw_topic_data = req.data

src/ros_test_pkg/CMakeLists.txt

@@ -127,7 +127,7 @@ include_directories(
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -145,7 +145,8 @@ add_executable(subscribe_node src/topic_subscriber.cpp)
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
+add_dependencies(publish_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(subscribe_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 ## Specify libraries to link a library or executable target against
 target_link_libraries(publish_node
   ${catkin_LIBRARIES}