将旧仓库中代码迁移至该新仓库中

README.md

@@ -1,3 +1,15 @@
-# arduino_robot_arm
+#arduino_robot_arm
+Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+Author: corvin
 
-该项目是使用arduino作为下位机主控板,普通的三线舵机作为 驱动器制作的一个低成本7轴机械臂.整个机械臂的外观结构使 用瓦楞纸,这样结构就容易修改且轻便.
+#Description:
+  该项目是使用arduino作为下位机主控板,普通的三线舵机作为
+  驱动器制作的一个低成本7轴机械臂.整个机械臂的外观结构使
+  用瓦楞纸,这样结构就容易修改且轻便.
+
+#Usage:
+  1.首先运行scripts目录下的setup_env.sh脚本来安装配置编译
+    环境,该脚本只需要在下载好代码后执行一次即可.
+
+#History:
+  20190122: Initial this readme file.

arduino_code/arduino_robot_arm/arduino_robot_arm.ino

@@ -0,0 +1,160 @@
+/********************************************************************************
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+       使用dfrobot的arduino UNO开发板，再接扩展板。所有的舵机都接在扩展板上,这样方便接线.
+    当然其他公司的UNO板也行，在控制转动前需要先使能指定pwm引脚上的舵机，否则发送转动舵机的命
+    令是不执行的.具体操作命令格式如下：
+    (0).使能pwm 2号引脚上的舵机,该引脚舵机索引值为0,第2个参数表明使能（非0）或禁用（0）：
+      e 0 1
+    (1).控制舵机旋转，舵机索引分别为为0，1。后面接着要转动到的角度，最后的数字为移动的延时参数:
+      w 0 90 20:该串口命令中w表示写入，0表示0号舵机，90表示旋转到90度,每次移动延时20ms
+      w 1 30 50:表示1号舵机移动到30度,每个移动周期延时50ms.
+    (2).读取指定舵机ID的当前旋转到的角度：
+      r 0:串口输入该命令，则返回90,表示0号舵机当前在90度位置.
+      r 1:输入该命令后，返回30,表示1号舵机当前在30度位置。
+    (3).获取所有舵机的使能状态命令s:
+      s:0 0 0 1 0 0：返回的一串数字表明第几个舵机使能。
+    (4).获取当前所有舵机的角度p:
+      p:0 0 0 90 0 0:第4个舵机当前在90度位置。
+  History:
+    20181225: initial this code.
+********************************************************************************/
+#define  FIRMWARE_VERSION   1
+#define  CONNECT_BAUDRATE   57600
+
+#include <Servo.h>
+#include "serialData.h"
+#include "commands.h"
+#include "servos.h"
+
+/* Run a command.  Commands are defined in commands.h */
+void runCommand(void)
+{
+  serialObj.arg1 = atoi(serialObj.argv1);
+  serialObj.arg2 = atoi(serialObj.argv2);
+  serialObj.arg3 = atoi(serialObj.argv3);
+
+  switch (serialObj.cmd_chr)
+  {
+    case GET_CONNECT_BAUDRATE:  // 'b':get connect baud rate.
+      Serial.println(CONNECT_BAUDRATE);
+      break;
+
+    case SET_ONE_SERVO_ENABLE:  // 'e':enable one servo.
+      myServos[serialObj.arg1].setEnable(serialObj.arg2);
+      Serial.println("OK");
+      break;
+
+    case SET_ONE_SERVO_POS:   // 'w':set one servo target position.
+      myServos[serialObj.arg1].setTargetPos(serialObj.arg2, serialObj.arg3);
+      Serial.println("OK");
+      break;
+
+    case GET_ONE_SERVO_POS:   // 'r':get one servo current position.
+      Serial.println(myServos[serialObj.arg1].getServoObj().read());
+      break;
+
+    case GET_ALL_SERVOS_POS: // 'p':get all servos current position.
+      for (byte i = 0; i < N_SERVOS; i++)
+      {
+        Serial.print(myServos[i].getCurrentPos());
+        Serial.print(' ');
+      }
+      Serial.println("");
+      break;
+
+    case GET_ALL_SERVOS_ENABLE: // 's':get all servo enable status.
+      for (byte i = 0; i < N_SERVOS; i++)
+      {
+        Serial.print(myServos[i].isEnabled());
+        Serial.print(' ');
+      }
+      Serial.println("");
+      break;
+
+    case GET_FIRMWARE_VERSION:  // 'v':get current firmware version.
+      Serial.println(FIRMWARE_VERSION);
+      break;
+
+    default:
+      Serial.println("Invalid Command");
+      break;
+  }
+}
+
+/* Setup function--runs once at startup. */
+void setup()
+{
+  Serial.begin(CONNECT_BAUDRATE);
+  serialObj.resetCmdParam();
+
+  /* when power on init all servos position */
+  for (byte i = 0; i < N_SERVOS; i++)
+  {
+    myServos[i].initServo(myServoPins[i], servoInitPosition[i], 0);
+  }
+}
+
+/* Read and parse input from the serial port and run any valid commands. */
+void loop()
+{
+  while (Serial.available() > 0)
+  {
+    char tmp_chr = Serial.read(); //Read the next character
+
+    if (tmp_chr == 13) // Terminate a command with a CR
+    {
+      runCommand();
+      serialObj.resetCmdParam();
+    }
+    else if (tmp_chr == ' ') // Use spaces to delimit parts of the command
+    {
+      if (serialObj.argCnt == 0)
+      {
+        serialObj.argCnt++;
+      }
+      else if (serialObj.argCnt == 1)
+      {
+        serialObj.argCnt++;
+        serialObj.argIndex = 0;
+      }
+      else if (serialObj.argCnt == 2)
+      {
+        serialObj.argCnt++;
+        serialObj.argIndex = 0;
+      }
+    }
+    else //parse valid param
+    {
+      if (serialObj.argCnt == 0) // The first arg is the char command
+      {
+        serialObj.cmd_chr = tmp_chr;
+      }
+      else if (serialObj.argCnt == 1) // Get char cmd's first param
+      {
+        serialObj.argv1[serialObj.argIndex] = tmp_chr;
+        serialObj.argIndex++;
+      }
+      else if (serialObj.argCnt == 2)
+      {
+        serialObj.argv2[serialObj.argIndex] = tmp_chr;
+        serialObj.argIndex++;
+      }
+      else if (serialObj.argCnt == 3)
+      {
+        serialObj.argv3[serialObj.argIndex] = tmp_chr;
+        serialObj.argIndex++;
+      }
+    }
+  }//end while
+
+  // Check everyone servo if isEnabled, when true will move servo. Other don't move servo.
+  for (byte i = 0; i < N_SERVOS; i++)
+  {
+    if (myServos[i].isEnabled())
+    {
+      myServos[i].moveServo();
+    }
+  }
+}//end loop

arduino_code/arduino_robot_arm/commands.h

@@ -0,0 +1,20 @@
+/**********************************************************************
+ Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+   定义单个字符的命令,该命令需要通过arduino串口发送过来.
+ History:
+   20181225: initial this code.
+*/
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#define  GET_CONNECT_BAUDRATE   'b'
+#define  SET_ONE_SERVO_ENABLE   'e'
+#define  SET_ONE_SERVO_POS      'w'
+#define  GET_ONE_SERVO_POS      'r'
+#define  GET_ALL_SERVOS_POS     'p'
+#define  GET_ALL_SERVOS_ENABLE  's'
+#define  GET_FIRMWARE_VERSION   'v'
+
+#endif

arduino_code/arduino_robot_arm/serialData.h

@@ -0,0 +1,38 @@
+/***************************************************************
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    用于从arduino的串口获得命令的类，包含各种成员变量和函数.
+  History:
+   20181225: initial this file.
+*************************************************************/
+#ifndef _SERIALDATA_H_
+#define _SERIALDATA_H_
+
+#define  ENTER_CHAR  '\r'
+
+class serialData
+{
+  public:
+    void resetCmdParam(void);
+
+    //argCnt:arg total cnt; argIndex:arg current index.
+    byte argCnt;
+    byte argIndex;
+
+    // Variable to hold the current single-character command
+    char cmd_chr;
+
+    // Character arrays to hold the first, second and third arguments
+    char argv1[4];
+    char argv2[4];
+    char argv3[4];
+
+    // The arguments converted to integers
+    int arg1;
+    int arg2;
+    int arg3;
+};
+
+serialData serialObj;
+#endif

arduino_code/arduino_robot_arm/serialData.ino

@@ -0,0 +1,19 @@
+/***************************************************************
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    用于arduino的串口获得命令类中函数的实现.
+  History:
+   20181225: initial this file.
+*************************************************************/
+void serialData::resetCmdParam(void)
+{
+  this->cmd_chr = ENTER_CHAR;
+  
+  memset(this->argv1, ENTER_CHAR, sizeof(this->argv1));
+  memset(this->argv2, ENTER_CHAR, sizeof(this->argv2));
+  memset(this->argv3, ENTER_CHAR, sizeof(this->argv3));
+
+  this->argCnt   = 0;
+  this->argIndex = 0;
+}

arduino_code/arduino_robot_arm/servos.h

@@ -0,0 +1,48 @@
+/***************************************************************
+ Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+ Author: corvin
+ Description:
+    Define servo class and servo pin etc. 
+ History:
+   20181128: initial this comment.
+   20181130: 新增了舵机使能状态的标识，而且最多可接12个舵机。
+   20181204: 新增getCurrentPos()函数用户获取当前舵机的角度。
+*************************************************************/
+#ifndef SERVOS_H
+#define SERVOS_H
+
+#define  N_SERVOS  6
+
+#define  SERVO_ENABLE   1
+#define  SERVO_DISABLE  0
+
+// Define All Servos's Pins
+byte myServoPins[N_SERVOS] = {3, 5, 6, 9, 10, 11};
+
+// Initial Servo Position [0, 160] degrees
+byte servoInitPosition[N_SERVOS] = {80, 150, 160, 80, 20, 80};
+
+class SweepServo
+{
+  public:
+    SweepServo();
+    void initServo(byte servoPin, unsigned int initPosition, unsigned int stepDelayMs);
+    void setTargetPos(unsigned int targetPos, unsigned int stepDelayMs);
+    byte getCurrentPos(void);
+    void setEnable(byte flag);
+    void moveServo(void);
+    byte isEnabled(void);
+    Servo getServoObj();
+
+  private:
+    Servo servo;
+    unsigned int stepDelayMs;
+    unsigned long lastMoveTime;
+    byte currentPosDegrees;
+    byte targetPosDegrees;
+    byte enabled;
+};
+
+SweepServo myServos[N_SERVOS];
+
+#endif

arduino_code/arduino_robot_arm/servos.ino

@@ -0,0 +1,91 @@
+/**********************************************************************
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+   Sweep servos one degree step at a time with a user defined
+   delay in between steps.  Supports changing direction mid-sweep.
+  History:
+  20181225: initial this code.
+**********************************************************************/
+// Constructor function
+SweepServo::SweepServo()
+{
+  this->currentPosDegrees = 0;
+  this->targetPosDegrees  = 0;
+  this->lastMoveTime      = 0;
+}
+
+// Init servo params, default all servos disabled.
+void SweepServo::initServo(byte servoPin, unsigned int initPosition, unsigned int stepDelayMs)
+{
+  this->servo.attach(servoPin);
+  this->stepDelayMs = stepDelayMs;
+  this->currentPosDegrees = initPosition;
+  this->targetPosDegrees  = initPosition;
+  this->lastMoveTime = millis();
+  this->enabled = SERVO_DISABLE;
+
+  this->servo.write(initPosition); //when power on, move all servos to initPosition
+}
+
+//Servo Perform Sweep
+void SweepServo::moveServo(void)
+{
+  // Get ellapsed time from last cmd time to now.
+  unsigned int delta = millis() - this->lastMoveTime;
+
+  // Check if time for a step
+  if (delta > this->stepDelayMs)
+  {
+    // Check step direction
+    if (this->targetPosDegrees > this->currentPosDegrees)
+    {
+      this->currentPosDegrees++;
+      this->servo.write(this->currentPosDegrees);
+    }
+    else if (this->targetPosDegrees < this->currentPosDegrees)
+    {
+      this->currentPosDegrees--;
+      this->servo.write(this->currentPosDegrees);
+    }
+    // if target == current position, do nothing
+
+    // reset timer, save current time to last cmd time.
+    this->lastMoveTime = millis();
+  }
+}
+
+// Set a new target position with step delay param.
+void SweepServo::setTargetPos(unsigned int targetPos, unsigned int stepDelayMs)
+{
+  this->targetPosDegrees = targetPos;
+  this->stepDelayMs      = stepDelayMs;
+}
+
+byte SweepServo::getCurrentPos(void)
+{
+  return this->currentPosDegrees;
+}
+
+void SweepServo::setEnable(byte flag)
+{
+  if (flag == SERVO_ENABLE)
+  {
+    this->enabled = SERVO_ENABLE;
+  }
+  else
+  {
+    this->enabled = SERVO_DISABLE;
+  }
+}
+
+byte SweepServo::isEnabled(void)
+{
+  return this->enabled;
+}
+
+// Accessor for servo object
+Servo SweepServo::getServoObj()
+{
+  return this->servo;
+}

ros_code/.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

ros_code/src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

ros_code/src/arduino_servo_connect/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arduino_servo_connect)
+
+## 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
+  dynamic_reconfigure
+  message_generation
+  rospy
+  std_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
+   MoveServo.msg
+#   Message2.msg
+)
+
+## Generate services in the 'srv' folder
+add_service_files(
+    FILES
+    ServoRead.srv
+    ServoWrite.srv
+    ServoEnable.srv
+    GetAllServoPos.srv
+    GetAllServoEnable.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
+)
+
+################################################
+## 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/arduino_params.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 arduino_servo_connect
+#  CATKIN_DEPENDS message_generation rospy std_msgs
+#  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}/arduino_servo_connect.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}_node src/arduino_servo_connect_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})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_arduino_servo_connect.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)

ros_code/src/arduino_servo_connect/cfg/arduino_params.cfg

@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    人脸跟踪时arduino节点的可以调用dynamic_reconfigure动态更新参数.
+  History:
+    20181225: initial this file.
+'''
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = "arduino_servo_connect"
+gen = ParameterGenerator()
+
+gen.add("firmware_pub_rate", double_t, 0, "pub arduino firmware ver rate", 0.1, 0.01, 1.0)
+gen.add("servo_move_delay", int_t, 0, "when exit move servo delay", 50, 0, 150)
+
+exit(gen.generate(PACKAGE, "arduino_dynamic_reconfig", "arduino_servo"))

ros_code/src/arduino_servo_connect/cfg/config_params.yaml

@@ -0,0 +1,20 @@
+##################################################################################
+# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+# Author: corvin
+# Description:配置连接的arduino的参数,包括arduino端口号,波特率,连接超时时间等.
+#   port: arduino控制板的端口号.
+#   baud: 连接arduino的串口波特率.
+#   timeout: 串口读取数据时超时时间.
+#   servo_cnt: 当前可以控制的舵机数量,这里默认为6,暂不需要修改.
+#   servo_move_delay: 断开arduino连接时,舵机复位时移动的延时,单位ms.
+#   version_pub_rate: arduino控制舵机移动的固件代码版本通过话题发布的频率.
+# History:
+#   20181225: initial this config file.
+###################################################################################
+port: /dev/ttyACM0
+baud: 57600
+timeout: 0.5
+servo_cnt: 6
+servo_move_delay: 50
+version_pub_rate: 0.1
+default_degrees: [80, 150, 160, 80, 20, 80]

ros_code/src/arduino_servo_connect/launch/arduino_servo_connect.launch

@@ -0,0 +1,24 @@
+<!--
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    连接arduino,这样才能控制舵机移动.同时加载一些连接arduino的配置参数,
+    连接舵机的一些参数.
+  History:
+    20181225: initial this code.
+-->
+<launch>
+  <arg name="MOVE_SERVO_TOPIC" default="/arduino_servo_node/moveServoTopic" />
+  <arg name="FIRMWARE_VER_TOPIC" default="/arduino_servo_node/firmwareVersionTopic"/>
+
+  <!-- startup arduino servo node dynamic reconfigure server -->
+  <node name="arduino_dynamic_reconfig" pkg="arduino_servo_connect" type="dynamic_server.py" output="screen" />
+
+  <!-- startup arduino servo connect node -->
+  <node name="arduino_servo_node" pkg="arduino_servo_connect" type="arduino_node.py" output="screen">
+    <rosparam file="$(find arduino_servo_connect)/cfg/config_params.yaml" command="load"/>
+
+    <param name="move_servo_topic" value="$(arg MOVE_SERVO_TOPIC)" />
+    <param name="firmware_version_topic" value="$(arg FIRMWARE_VER_TOPIC)" />
+  </node>
+</launch>

ros_code/src/arduino_servo_connect/msg/MoveServo.msg

@@ -0,0 +1,4 @@
+# Move Servo msg, include three param: servoid, targetPos, delay
+uint8 servoId
+uint16 targetPos
+uint8 delay

ros_code/src/arduino_servo_connect/package.xml

@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>arduino_servo_connect</name>
+  <version>0.0.0</version>
+  <description>The arduino_servo_connect package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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/arduino_servo_connect</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>message_generation</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>dynamic_reconfigure</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/arduino_servo_connect/src/arduino_driver.py

@@ -0,0 +1,255 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    A Python driver for the Arduino microcontroller.
+  History:
+      20181225: initial this comment.
+"""
+
+import rospy
+import thread,serial,random
+import os, sys, time, traceback
+from exceptions import Exception
+from serial.serialutil import SerialException, SerialTimeoutException
+
+
+SERVO_COUNT = 6
+
+class CmdErrCode:
+    SUCCESS = 0
+    TIMEOUT = 1
+    NOVALUE = 2
+    SERIALERROR = 3
+    ErrCodeStr = ['SUCCESS', 'TIMEOUT', 'NOVALUE', 'SERIALERROR']
+
+
+class CmdException(Exception):
+    def __init__(self, code):
+        self.code = code
+
+    def __str__(self):
+        return repr(self.code)
+
+
+class Arduino:
+    ''' Configuration Connect Parameters
+    '''
+    def __init__(self, port="/dev/ttyACM0", baudrate=57600, timeout=0.5):
+        self.port = port
+        self.baudrate = baudrate
+        self.timeout  = timeout
+        self.interCharTimeout = timeout / 30.
+        # Keep things thread safe
+        self.mutex = thread.allocate_lock()
+
+    ''' Ready to connect arduino port
+    '''
+    def connect(self):
+        try:
+            rospy.loginfo("Connecting to Arduino on port:%s ", self.port)
+            #The port has to be open once with default baud before opening again for real
+            self.serial_port = serial.Serial(port=self.port)
+            self.serial_port = serial.Serial(port=self.port, baudrate=self.baudrate, timeout=max(1.0, self.timeout))
+
+            #Take time for serial port to wake up
+            maxAttempts = 10
+            attempts = 0
+            timeout  = self.timeout
+
+            self.serial_port.write('\r')
+            time.sleep(timeout)
+            test = self.serial_port.read()
+
+            # Wakeup the serial port
+            while attempts < maxAttempts and test == '':
+                attempts += 1
+                self.serial_port.write('\r')
+                time.sleep(timeout)
+                test = self.serial_port.read()
+                rospy.loginfo("Waking up serial port attempts " + str(attempts) + "/" + str(maxAttempts))
+
+            if test == '':
+                raise SerialException
+
+            # Reset timeout to the user specified timeout
+            self.serial_port.timeout = self.timeout
+            self.serial_port.writeTimeout = self.timeout
+
+            attempts = 0
+            while self.get_baud() != self.baudrate and attempts < maxAttempts:
+                attempts += 1
+                self.serial_port.flushInput()
+                self.serial_port.flushOutput()
+                time.sleep(timeout)
+
+            try:
+                self.serial_port.inWaiting()
+                rospy.loginfo("Arduino connect successfully !")
+            except IOError:
+                raise SerialException
+        except SerialException:
+            rospy.logerr("Serial Exception:")
+            rospy.logerr(sys.exc_info())
+            traceback.print_exc(file=sys.stdout)
+            rospy.logerr("Can't connect to Arduino ! Make sure it is plugged:" + self.port)
+            return False
+
+        return True
+
+    def close_port(self):
+        self.serial_port.close()
+
+    def execute(self, cmd, timeout=0.5):
+        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
+        '''
+        self.mutex.acquire()
+
+        value = None
+        error = None
+        try:
+            start = time.time()
+            self.serial_port.flushInput()
+            self.serial_port.flushOutput()
+            self.serial_port.write(cmd + '\r')
+            value = self.serial_port.readline().strip('\n').strip('\r')
+        except SerialException:
+            self.logerr("cmd:" + str(cmd) + " filed with serial excption!")
+            error = CmdErrCode.SERIALERROR
+        except SerialTimeoutException:
+            self.logerr("cmd:" + str(cmd) + " time out!")
+            error = CmdErrCode.TIMEOUT
+
+        duration = time.time() - start
+
+        if error is None and (value is None or len(value) == 0):
+            duration = time.time() - start
+            if duration > timeout:
+                self.logerr("cmd:" + str(cmd) + " time out!")
+                error = CmdErrCode.TIMEOUT
+            else:
+                self.logerr("cmd:" + str(cmd) + " didn't return a value")
+                error = CmdErrCode.NOVALUE
+
+        self.mutex.release()
+        if error:
+            raise CmdException(error)
+
+        return value
+
+    def execute_ack(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
+        '''
+        try:
+            ack = self.execute(cmd)
+            return ack == "OK"
+        except CmdException as e:
+            return False
+
+    def execute_array(self, cmd):
+        try:
+            values = self.execute(cmd).split()
+        except Exception as e:
+            values = None
+            rospy.logerr("Error! In execute_array():%s", str(e))
+
+        return values
+
+    def get_baud(self):
+        ''' Get the current baud rate on the serial port.
+        '''
+        try:
+            return int(self.execute('b'));
+        except:
+            return None
+
+    def get_firmware_version(self):
+        ''' Get the current arduino firmware version.
+        '''
+        try:
+            return int(self.execute('v'));
+        except:
+            return None
+
+    def servo_enable(self, id, flag):
+        ''' Enable or Disable servo by id
+        '''
+        return self.execute_ack('e %d %d' %(id, flag))
+
+    def servo_write(self, servoId, pos, delay):
+        ''' Usage: servo_write(id, pos, delay)
+            Position is given in radians and converted to degrees before sending
+        '''
+        return self.execute_ack('w %d %d %d' %(servoId, pos, delay))
+
+    def servo_read(self, servoid):
+        ''' Usage: servo_read(id)
+            The returned position is converted from degrees to radians
+        '''
+        return int(self.execute('r %d' %servoid))
+
+    def getAllServosEnabled(self):
+        values = self.execute_array('s')
+
+        if len(values) != SERVO_COUNT:
+            return None
+        else:
+            return map(int, values)
+
+    def getAllServosPos(self):
+        values = self.execute_array('p')
+
+        if len(values) != SERVO_COUNT:
+            return None
+        else:
+            return map(int, values)
+
+""" Basic test for connectivity """
+if __name__ == "__main__":
+    portName = "/dev/ttyACM0"
+    baudRate = 57600
+    delay = 60
+    testTimes = 3
+
+    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
+    myArduino.connect()
+
+    print "Get arduino firmware version: " + str(myArduino.get_firmware_version())
+
+    print "Enable all servos..."
+    for i in range(SERVO_COUNT):
+        myArduino.servo_enable(i, 1)
+
+    print "Check if enable all servos ..."
+    status = []
+    status = myArduino.getAllServosEnabled()
+    for i in range(SERVO_COUNT):
+        if 1 != status[i]:
+            print "ERROR! " + str(status[i]) + " servo can't enable..."
+            myArduino.close_port()
+            os._exit(1)
+    print "All servos be enabled !"
+
+    print "Start to test robot arm..."
+    degrees = []
+
+    for i in range(testTimes):
+        for j in range(SERVO_COUNT):
+            random_deg = random.randint(0, 160)
+            print 'Servo:'+str(j)+" :"+str(random_deg)
+            myArduino.servo_write(j, random_deg, delay)
+
+        time.sleep(10)
+        degrees = myArduino.getAllServosPos()
+        print degrees
+
+    print ''
+    print "Disable all servo..."
+    for i in range(12):
+        myArduino.servo_enable(i, 0)
+
+    print "Connection test over ! Shutting down arduino ..."
+    myArduino.close_port()

ros_code/src/arduino_servo_connect/src/arduino_node.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+    A ROS Node for the Arduino microcontroller.
+  History:
+    20181225: initial this code.
+"""
+
+import os
+import rospy
+from std_msgs.msg import UInt16
+from arduino_driver import Arduino
+from arduino_servo_connect.srv import *
+from arduino_servo_connect.msg import *
+from serial.serialutil import SerialException
+import dynamic_reconfigure.client as DynamicReconfig
+
+class ArduinoROS():
+    def __init__(self):
+        rospy.init_node('arduino_servo_node', log_level=rospy.INFO)
+        self.dynamic_client = DynamicReconfig.Client("arduino_dynamic_reconfig",timeout=10,config_callback=self.dynamic_callBack)
+
+        # Get the actual node name in case it is set in the launch file
+        self.name = rospy.get_name()
+        rospy.on_shutdown(self.shutdown)
+
+        # Get all param
+        self.getAllParams()
+
+        #subscrib topic to move servo
+        rospy.Subscriber(self.moveServoTopic, MoveServo, self.moveServoCB)
+
+        #define publisher to pub arduino firmware version
+        self.versionPub = rospy.Publisher(self.firmwareVerTopic, UInt16, queue_size=1)
+
+        #define all services
+        self.defineAllServices()
+
+        # Initialize the controlller
+        self.controller = Arduino(self.port, self.baud, self.timeout)
+        self.controller.connect()
+        rospy.loginfo("Connected to Arduino on port:" + self.port + " at " + str(self.baud) + " baud")
+
+        # Enable servo x and servo y
+        self.enableAllServos()
+
+        # while loop
+        while not rospy.is_shutdown():
+            rate = rospy.Rate(self.pubRate)
+            firmwareVersion = self.controller.get_firmware_version()
+            self.versionPub.publish(firmwareVersion)
+            rate.sleep()
+
+    def enableAllServos(self):
+        for i in range(0, self.servoCnt):
+            self.controller.servo_enable(i, 1)
+
+    def dynamicReconfig(self, paramList):
+        self.pubRate = paramList['firmware_pub_rate']
+        self.move_delay    = paramList['servo_move_delay']
+        #rospy.loginfo("(%f,%d,%d,%d)" %(self.pubRate,self.move_delay))
+
+    def dynamic_callBack(self, config):
+        dynamic_param = self.dynamic_client.get_configuration(timeout=3)
+        self.dynamicReconfig(dynamic_param)
+
+    def defineAllServices(self):
+        # Two services to position/read postion a PWM servo
+        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
+        rospy.Service('~servo_read',  ServoRead,  self.ServoReadHandler)
+
+        # Service to enable a servo
+        rospy.Service('~servo_enable', ServoEnable, self.ServoEnableHandler)
+
+        # Two services to get all servos position and enabled status
+        rospy.Service('~get_all_servos_pos',    GetAllServoPos,    self.GetAllServoPosHandler)
+        rospy.Service('~get_all_servos_enable', GetAllServoEnable, self.GetAllServoEnableHandler)
+
+    def moveServoCB(self, msgData):
+        self.moveServo(msgData.servoId, msgData.targetPos, msgData.delay)
+
+    def getAllParams(self):
+        self.port = rospy.get_param("~port", "/dev/ttyACM0")
+        self.baud = int(rospy.get_param("~baud", 57600))
+        self.timeout = rospy.get_param("~timeout", 0.5)
+        self.servoCnt = rospy.get_param("~servo_cnt", 6)
+        self.pubRate = rospy.get_param("~version_pub_rate", 0.1)
+        self.move_delay = rospy.get_param("~servo_move_delay", 50)
+        self.moveServoTopic = rospy.get_param("~move_servo_topic", "/arduino_servo_node/moveServoTopic")
+        self.firmwareVerTopic = rospy.get_param("~firmware_version_topic", "/arduino_servo_node/firmwareVersionTopic")
+        self.default_degrees = rospy.get_param("~default_degrees", [80,150,160,80,20,80])
+
+    def initAllServosPose(self):
+        for i in range(0, self.servoCnt):
+            self.moveServo(i, self.default_degrees[i], self.move_delay)
+
+    # Service callback functions
+    def ServoWriteHandler(self, req):
+        self.controller.servo_write(req.id, req.value, req.delay)
+        return ServoWriteResponse()
+
+    def ServoReadHandler(self, req):
+        pos = self.controller.servo_read(req.id)
+        return ServoReadResponse(pos)
+
+    def ServoEnableHandler(self, req):
+        self.controller.servo_enable(req.id, req.flag)
+        return ServoEnableResponse()
+
+    def GetAllServoPosHandler(self, req):
+        positions = []
+        positions = self.controller.getAllServosPos()
+        return GetAllServoPosResponse(positions)
+
+    def GetAllServoEnableHandler(self, req):
+        enabled = []
+        enabled = self.controller.getAllServosEnabled()
+        return GetAllServoEnableResponse(enabled)
+
+    def moveServo(self, servo_id, move_angular, move_delay):
+        self.controller.servo_write(servo_id, move_angular, move_delay)
+
+    def enableServoByID(self, servo_id, flag):
+        self.controller.servo_enable(servo_id, flag)
+
+    def shutdown(self):
+        rospy.logwarn("Shutting down arduino_servo_node ...")
+        # init all servoes position to origin
+        self.initAllServosPose()
+
+        # Close the serial port
+        try:
+            self.controller.close()
+        except:
+            pass
+        finally:
+            rospy.logwarn("Arduino serial port closed.")
+            os._exit(0)
+
+if __name__ == '__main__':
+    try:
+        myArduino = ArduinoROS()
+    except SerialException:
+        rospy.logerr("Serial exception trying to open port.")
+        os._exit(1)
+

ros_code/src/arduino_servo_connect/src/dynamic_server.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+      arduino节点代码可以使用dynamic_reconfigure来动态更新
+    节点参数。
+  History:
+      20181225: initial this file.
+'''
+
+import rospy
+
+from dynamic_reconfigure.server import Server
+from arduino_servo_connect.cfg import arduino_servoConfig
+
+def callback(config, level):
+    rospy.loginfo("pub_rate:{firmware_pub_rate},move_delay:{servo_move_delay}".format(**config))
+    return config
+
+if __name__ == "__main__":
+    rospy.init_node("arduino_dynamic_reconfig", anonymous=False)
+
+    Server(arduino_servoConfig, callback)
+    rospy.spin()

ros_code/src/arduino_servo_connect/srv/GetAllServoEnable.srv

@@ -0,0 +1,2 @@
+---
+int32[] enables

ros_code/src/arduino_servo_connect/srv/GetAllServoPos.srv

@@ -0,0 +1,2 @@
+---
+int32[] positions

ros_code/src/arduino_servo_connect/srv/ServoEnable.srv

@@ -0,0 +1,3 @@
+uint8 id
+uint8 flag
+---

ros_code/src/arduino_servo_connect/srv/ServoRead.srv

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

ros_code/src/arduino_servo_connect/srv/ServoWrite.srv

@@ -0,0 +1,4 @@
+uint8 id
+int32 value
+uint8 delay
+---

ros_code/src/arm_keyboard_control/CMakeLists.txt

@@ -0,0 +1,197 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arm_keyboard_control)
+
+## 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
+  arduino_servo_connect
+)
+
+## 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 keyboard_control
+#  CATKIN_DEPENDS arduino_servo_connect/MoveServo
+#  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}/keyboard_control.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}_node src/keyboard_control_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})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_keyboard_control.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)

ros_code/src/arm_keyboard_control/package.xml

@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>arm_keyboard_control</name>
+  <version>0.0.0</version>
+  <description>The arm_keyboard_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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/keyboard_control</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>arduino_servo_connect</build_depend>
+  <build_export_depend>arduino_servo_connect</build_export_depend>
+  <exec_depend>arduino_servo_connect</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/arm_keyboard_control/src/arm_keyboard_control.py

@@ -0,0 +1,106 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+"""
+  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:
+  History:
+      20181226: initial this code.
+"""
+
+from __future__ import print_function
+
+import roslib; roslib.load_manifest('arm_keyboard_control')
+import rospy
+
+from arduino_servo_connect.srv import *
+from arduino_servo_connect.msg import MoveServo
+import sys, select, termios, tty
+
+msg = r"""
+Reading from the keyboard and Publishing to MoveServo topic!
+--------------------------------------------------------------
+            Joint 2:[y  u]
+           //            \\
+          //              \\
+         //               Joint 3:[i  o] = Joint 4:[k  l] = Joint 5:[, .]
+  Joint 1:[h  j]
+       ||
+       ||
+  Joint 0:[n  m]
+---------------------------------------------------------------
+
+CTRL-C to quit
+"""
+
+pub = rospy.Publisher('/arduino_servo_node/moveServoTopic', MoveServo, queue_size = 1)
+Degrees = []
+
+keyArray = {
+    'n':(0, 1), 'm':(0, 0),
+    'h':(1, 1), 'j':(1, 0),
+    'y':(2, 1), 'u':(2, 0),
+    'i':(3, 1), 'o':(3, 0),
+    'k':(4, 1), 'l':(4, 0),
+    ',':(5, 1), '.':(5, 0),
+}
+
+def getKey():
+    tty.setraw(sys.stdin.fileno())
+    select.select([sys.stdin], [], [], 0)
+    key = sys.stdin.read(1)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+    return key
+
+def sendMoveMsg(servoId, targetPos, delay):
+        moveMsg = MoveServo()
+        moveMsg.servoId = servoId
+        moveMsg.targetPos = targetPos
+        moveMsg.delay = delay
+        pub.publish(moveMsg)
+
+def calTargetPose(servoId, turnFlag):
+    targetPose = Degrees[servoId]
+    if turnFlag == 0:
+        if targetPose > 0:
+            targetPose -= 1
+    else:
+        if targetPose < 160:
+            targetPose += 1
+
+    return targetPose
+
+if __name__=="__main__":
+    settings = termios.tcgetattr(sys.stdin)
+    rospy.init_node('robotArm_keyboard_control_node')
+    getServoPoseService = rospy.ServiceProxy("/arduino_servo_node/get_all_servos_pos", GetAllServoPos)
+    moveDelay = rospy.get_param("~delay", 40)
+    status = 0
+
+    try:
+        print(msg)
+
+        while(1):
+            key = getKey()
+
+            if key in keyArray.keys():
+                servoID = keyArray[key][0]
+                turnFlag = keyArray[key][1]
+
+                servoDegrees = getServoPoseService()
+                Degrees = servoDegrees.positions
+                targetPose = calTargetPose(servoID, turnFlag)
+
+                if (status == 24):
+                    print(msg)
+                status = (status + 1) % 25
+            else:
+                if (key == '\x03'):
+                    break
+            rospy.loginfo("servoID:" + str(servoID) + ", targetPose:" + str(targetPose))
+            sendMoveMsg(servoID, targetPose, moveDelay)
+    except Exception as e:
+        print(e)
+    finally:
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

ros_code/src/robot_arm_bringup/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_arm_bringup)
+
+## 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)
+
+## 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_arm_bringup
+#  CATKIN_DEPENDS other_catkin_pkg
+#  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_arm_bringup.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}_node src/robot_arm_bringup_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})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_arm_bringup.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)

ros_code/src/robot_arm_bringup/launch/robot_arm_bringup.launch

@@ -0,0 +1,22 @@
+<!--
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description: 该launch文件为arduino机械臂最先需要启动的launch文件,当启动该文件后，机械臂
+      就基本上进入了正常工作模式.
+  History:
+      20190104: Initial this launch file.
+      20190108: 新增启动moveIt相关的launch文件,发布机械臂当前关节状态,方便与rviz中仿真同步.
+-->
+<launch>
+  <!-- 1.startup arduino_servo_connect node -->
+  <include file="$(find arduino_servo_connect)/launch/arduino_servo_connect.launch" />
+
+  <!-- 2.startup robot_arm moveit config launch -->
+  <include file="$(find robot_arm_moveit_config)/launch/robot_arm.launch" />
+
+  <!-- 3.startup joint_state publish node -->
+  <node pkg="robot_arm_control" type="pub_joint_state.py" name="pub_joint_state_node" output="screen" />
+
+  <!-- 4.startup joint trajectory action servo node -->
+  <node pkg="robot_arm_control" type="follow_joint_trajectory.py" name="follow_trajectory_node" output="screen" />
+</launch>

ros_code/src/robot_arm_bringup/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_arm_bringup</name>
+  <version>0.0.0</version>
+  <description>The robot_arm_bringup package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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_arm_bringup</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>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/robot_arm_control/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_arm_control)
+
+## 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
+  actionlib
+  moveit_msgs
+  roscpp
+  rospy
+  std_msgs
+  sensor_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
+#   moveit_msgs#   std_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
+  CATKIN_DEPENDS actionlib moveit_msgs roscpp rospy std_msgs sensor_msgs
+#  LIBRARIES robot_arm_control
+#  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_arm_control.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}_node src/robot_arm_control.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})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_arm_control.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)

ros_code/src/robot_arm_control/package.xml

@@ -0,0 +1,79 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_arm_control</name>
+  <version>0.0.0</version>
+  <description>The robot_arm_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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_arm_control</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>actionlib</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>moveit_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/robot_arm_control/src/arm_joint.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+# _*_ coding: utf-8 _*_
+
+'''
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description: 这个代码主要就是为了将从moveIt生成的控制舵机移动的命令给转换下区间,
+    由于生成的控制命令序列有些包含负数,所以需要将其转换到0-160度范围内servo才能执行.
+  History:
+    20190110: Initial this file.
+'''
+
+import roslib
+import rospy
+from arduino_servo_connect.msg import MoveServo
+
+
+class ARMJoint:
+    def __init__(self, name, servoID):
+        self.name = name
+        self.servoID = servoID
+        self.delay = 40
+        self.targetPose = 0
+        self.servoPub = rospy.Publisher("/arduino_servo_node/moveServoTopic", MoveServo, queue_size=5)
+
+
+    def moveServoTarget(self):
+        self.checkTargetPose()
+
+        moveServoMsg = MoveServo()
+        moveServoMsg.servoId = self.servoID
+        moveServoMsg.targetPos = self.targetPose
+        moveServoMsg.delay = self.delay
+        self.servoPub.publish(moveServoMsg)
+        #rospy.logwarn("ServoID:" + str(self.servoID) + ",TargetPose:" + str(self.targetPose))
+
+
+    def checkTargetPose(self):
+        if self.servoID == 0:
+            self.targetPose += 80
+        elif self.servoID == 1:
+            self.targetPose = abs(self.targetPose - 80)
+        elif self.servoID == 2:
+            self.targetPose += 80
+        elif self.servoID == 3:
+            self.targetPose += 80
+        elif self.servoID == 4:
+            self.targetPose = abs(self.targetPose - 80)
+        else:
+            rospy.logerr("ServoID error:"+str(self.servoID))
+

ros_code/src/robot_arm_control/src/follow_joint_trajectory.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+'''
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description:该文件为启动机械臂运动控制的ActionServer，用于接收moveIt!的action client
+    请求,然后根据请求中的goal，来控制机械臂的舵机按照trajectory进行移动到达goal.
+  History:
+      20190110: Initial this file.
+'''
+
+import rospy, actionlib
+from math import degrees
+from arm_joint import ARMJoint
+from control_msgs.msg import FollowJointTrajectoryAction
+
+
+class ExecuteTrajectoryController:
+    def __init__(self, name):
+        self.name = name
+        rospy.init_node(self.name)
+
+        self.servo0_joint = ARMJoint('servo0_joint', 0)
+        self.servo1_joint = ARMJoint('servo1_joint', 1)
+        self.servo2_joint = ARMJoint('servo2_joint', 2)
+        self.servo3_joint = ARMJoint('servo3_joint', 3)
+        self.servo4_joint = ARMJoint('servo4_joint', 4)
+
+        self.joints = [ self.servo0_joint, self.servo1_joint, self.servo2_joint,
+                        self.servo3_joint, self.servo4_joint]
+
+        #startup action server
+        self.server = actionlib.SimpleActionServer('/robot_arm_controller/follow_joint_trajectory',
+                                                   FollowJointTrajectoryAction,
+                                                   execute_cb = self.actionServerCB,
+                                                   auto_start = False)
+        self.server.start()
+        rospy.loginfo("Start robot_arm_controller action server...")
+
+
+    def actionServerCB(self, goal):
+        rospy.loginfo("Robot Arm Action Server Get Goal!")
+        traj = goal.trajectory
+        #rospy.logwarn("Print all trajectory joint name:")
+        #rospy.logwarn(" ".join(traj.joint_names))
+
+        if not traj.points:
+            msg = "Trajectory points empty !"
+            rospy.logerr(msg)
+            self.server.set_aborted(text=msg)
+            return
+
+        if self.executeTrajectory(traj):
+            self.server.set_succeeded()
+        else:
+            self.server.set_aborted(text="Execution failed !")
+        rospy.loginfo(self.name + ": Done.")
+
+
+    def executeTrajectory(self, traj):
+        rospy.loginfo("Now Executing Joint trajectory ...")
+        try:
+            indexes = [traj.joint_names.index(joint.name) for joint in self.joints]
+        except ValueError as val:
+            rospy.logerr("Invalid joint in trajectory val = " + str(val))
+            return False
+
+        start = traj.header.stamp
+        if start.secs == 0 and start.nsecs == 0:
+            start = rospy.Time.now()
+
+        for point in traj.points:
+            desired = [point.positions[k] for k in indexes]
+            for i in indexes:
+                self.joints[i].targetPose = int(degrees(desired[i]))
+                self.joints[i].moveServoTarget()
+                rospy.sleep(0.03)
+
+        return True
+
+
+if __name__ == '__main__':
+    try:
+        ExecuteTrajectoryController('follow_controller')
+    except rospy.ROSInterruptException as e:
+        rospy.logerr("Failed to start follow controller:"+str(e))

ros_code/src/robot_arm_control/src/pub_joint_state.py

@@ -0,0 +1,80 @@
+#!/usr/bin/env python
+# _*_ coding:utf-8 _*_
+
+import roslib
+import rospy,os,sys
+from math import radians
+from arduino_servo_connect.srv import GetAllServoPos
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Header
+
+class PubJointState():
+    def __init__(self):
+        rospy.init_node('pub_joint_state_node', log_level=rospy.INFO)
+        self.name = rospy.get_name()
+        rospy.on_shutdown(self.shutdown)
+        rospy.sleep(5)
+
+        rospy.wait_for_service("/arduino_servo_node/get_all_servos_pos")
+        try:
+            self.getServoPoseSrv = rospy.ServiceProxy("/arduino_servo_node/get_all_servos_pos", GetAllServoPos)
+        except rospy.ServiceException as e:
+            rospy.logerr("Service call error:"+str(e))
+
+        self.pub_joint = rospy.Publisher('/move_group/controller_joint_states', JointState, queue_size=10)
+        self.rate = rospy.Rate(10)
+        self.allServoDeg = ['','','','','']
+        self.joint_msg = JointState()
+        self.joint_msg.header = Header()
+        self.joint_msg.name = ['servo0_joint','servo1_joint','servo2_joint','servo3_joint','servo4_joint']
+
+        while not rospy.is_shutdown():
+            servoDegSrv = self.getServoPoseSrv()
+
+            for i in range(0, 5):
+                self.allServoDeg[i] = radians(servoDegSrv.positions[i])
+                #rospy.loginfo(str(self.allServoDeg[i]))
+
+            self.procServoDeg()
+            self.pubServoPose()
+            self.rate.sleep()
+            #rospy.loginfo("----------------------------")
+
+    def procServoDeg(self):
+        #servo 0
+        self.allServoDeg[0] -= 1.396263;
+
+        #servo 1
+        if self.allServoDeg[1] >= 0 and self.allServoDeg[1] <= 1.396263:
+            self.allServoDeg[1] = abs(self.allServoDeg[1] - 1.396263)
+        else:
+            self.allServoDeg[1] = -abs(self.allServoDeg[1] - 1.396263)
+
+        #servo 2
+        self.allServoDeg[2] -= 1.396263
+
+        #servo 3
+        self.allServoDeg[3] -= 1.396263
+
+        #servo 4
+        if self.allServoDeg[4] >= 0 and self.allServoDeg[4] <= 1.396263:
+            self.allServoDeg[4] = abs(self.allServoDeg[4] - 1.396263)
+        else:
+            self.allServoDeg[4] = -abs(self.allServoDeg[4] - 1.396263)
+
+    def pubServoPose(self):
+        self.joint_msg.header.stamp = rospy.Time.now()
+        self.joint_msg.position = self.allServoDeg
+        self.pub_joint.publish(self.joint_msg)
+
+
+    def shutdown(self):
+        rospy.logwarn("shutdown pub_joint_status_node now ...")
+
+
+if __name__ == '__main__':
+    try:
+        myJointState = PubJointState()
+    except Exception as e:
+        rospy.logerr("ErrorMsg: " + str(e))
+        os._exit(1)

ros_code/src/robot_arm_control/src/robot_arm_control.cpp

@@ -0,0 +1,67 @@
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <control_msgs/FollowJointTrajectoryActionGoal.h>
+#include <control_msgs/FollowJointTrajectoryActionResult.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <sensor_msgs/JointState.h>
+#include <iostream>
+#include <string.h>
+#include <stdio.h>
+#include <vector>
+
+using namespace std;
+
+class FollowJointTrajectoryAction
+{
+    public:
+        FollowJointTrajectoryAction(string name):
+            as_(nh_, name, false), action_name_(name)
+    {
+        as_.registerGoalCallback(boost::bind(&FollowJointTrajectoryAction::goalCB, this));
+        as_.registerPreemptCallback(boost::bind(&FollowJointTrajectoryAction::preemptCB, this));
+        as_.start();
+        ROS_INFO("Follow Joint Trajectory Action start...");
+    }
+
+    ~FollowJointTrajectoryAction(void)
+    {
+
+    }
+
+    void goalCB()
+    {
+        ROS_INFO("Now in joint trajectory goal callback function...");
+        if(as_.isNewGoalAvailable())
+        {
+
+        }
+        else
+        {
+            ROS_WARN("Goal isn't available !");
+        }
+
+        control_msgs::FollowJointTrajectoryResult result;
+        result.error_code = 0;
+        as_.setSucceeded(result);
+    }
+
+    void preemptCB()
+    {
+        ROS_WARN("%s:Preempted", action_name_.c_str());
+        as_.setPreempted();
+    }
+
+    protected:
+        ros::NodeHandle nh_;
+        actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
+        string action_name_;
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "robot_arm_action_server_node");
+    FollowJointTrajectoryAction followJoint("robot_arm_controller/follow_joint_trajectory");
+    ros::spin();
+
+    return 0;
+}

ros_code/src/robot_arm_description/CMakeLists.txt

@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_arm_description)
+
+## 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)
+
+## 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_arm_description
+#  CATKIN_DEPENDS other_catkin_pkg
+#  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_arm_description.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}_node src/robot_arm_description_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})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_arm_description.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)

ros_code/src/robot_arm_description/config/urdf.rviz

@@ -0,0 +1,179 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /RobotModel1
+        - /RobotModel1/Links1
+      Splitter Ratio: 0.555556
+    Tree Height: 677
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        arm_base_link:
+          Alpha: 0.8
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        down_arm_link:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo0:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo1:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo2:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo3:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo4:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        servo4_link:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        turn_panel:
+          Alpha: 0.6
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        up_arm_link:
+          Alpha: 0.7
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: arm_base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 0.835716
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.100509
+        Y: 0.0832568
+        Z: 0.144654
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.114796
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.59866
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 958
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000334fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000334000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000334fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000334000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006320000003efc0100000002fb0000000800540069006d0065010000000000000632000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004c20000033400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1586
+  X: -10
+  Y: 14

ros_code/src/robot_arm_description/launch/display.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="model" />
+  <arg name="gui" default="true" />
+    
+  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_arm_description)/model/arduino_robot_arm.urdf.xacro'" />
+  <param name="use_gui" value="$(arg gui)" />
+  
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
+  
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_arm_description)/config/urdf.rviz" />
+</launch>

ros_code/src/robot_arm_description/model/arduino_robot_arm.urdf

@@ -0,0 +1,488 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from arduino_robot_arm.urdf.xacro   | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="arduino_robot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+  <material name="blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="orange">
+    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
+  </material>
+  <material name="brown">
+    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
+  </material>
+  <material name="red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  <material name="yellow">
+    <color rgba="0.792156862745 0.819607843137 0.0 1.0"/>
+  </material>
+  <material name="gray">
+    <color rgba="0.752941176471 0.752941176471 0.752941176471 1.0"/>
+  </material>
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      
+    </plugin>
+  </gazebo>
+  <gazebo reference="cute_base_link">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part1">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Yellow</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part2">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part3">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Yellow</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part4">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part5">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Yellow</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part6">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="part7">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="claw1">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <gazebo reference="claw2">
+    <visual name="visual">
+      <material>
+        <script>
+          <uri>file://media/materials/scripts/gazebo.material</uri>
+          <name>Gazebo/Grey</name>
+        </script>
+      </material>
+    </visual>
+  </gazebo>
+  <transmission name="joint1_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint1_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint2_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint2_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint3_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint3_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint4_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint4_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint5_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint5_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint6_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint6_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="joint7_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="joint7">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="joint7_motor">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="claw_trans_1">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="claw">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="claw_motor_1">
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="world"/>
+  <joint name="world_base" type="fixed">
+    <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+    <parent link="world"/>
+    <child link="arm_base_link"/>
+  </joint>
+  <link name="arm_base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0.0325"/>
+      <geometry>
+        <cylinder length="0.065" radius="0.05"/>
+      </geometry>
+      <material name="gray"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0325"/>
+      <geometry>
+        <cylinder length="0.065" radius="0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="servo0">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="0.0096898" iyy="0.23735" iyz="0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.023 0.013 0.029"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.023 0.013 0.029"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo0_fix_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.0505"/>
+    <parent link="arm_base_link"/>
+    <child link="servo0"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <link name="turn_panel">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.002" radius="0.04"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.002" radius="0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo0_joint" type="revolute">
+    <origin xyz="0 0 0.0145"/>
+    <parent link="servo0"/>
+    <child link="turn_panel"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="120" lower="-1.4" upper="1.4" velocity="2.0"/>
+  </joint>
+  <link name="servo1">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.023 0.026"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.023 0.026"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo1_fix_joint" type="fixed">
+    <origin xyz="0 0 0.014"/>
+    <parent link="turn_panel"/>
+    <child link="servo1"/>
+  </joint>
+  <link name="up_arm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.04 0 0"/>
+      <geometry>
+        <box size="0.08 0.035 0.027"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.08 0.035 0.027"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo1_joint" type="revolute">
+    <origin rpy="0 -1.57079632679 0" xyz="0 0 0"/>
+    <parent link="servo1"/>
+    <child link="up_arm_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="120" lower="-1.4" upper="1.4" velocity="2.0"/>
+  </joint>
+  <link name="servo2">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.023 0.013"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.023 0.013"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo2_fix_joint" type="fixed">
+    <origin xyz="0.0655 0 0"/>
+    <parent link="up_arm_link"/>
+    <child link="servo2"/>
+  </joint>
+  <link name="down_arm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23735" ixy="-0.00045611" ixz="-0.0083689" iyy="0.23138" iyz="0.0096898" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0375 0 0"/>
+      <geometry>
+        <box size="0.075 0.035 0.027"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.075 0.035 0.027"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo2_joint" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="servo2"/>
+    <child link="down_arm_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="120" lower="-1.4" upper="1.4" velocity="2.0"/>
+  </joint>
+  <link name="servo3">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23138" ixy="0.00045611" ixz="-0.0096898" iyy="0.23735" iyz="-0.0083689" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.013 0.023"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.013 0.023"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo3_fix_joint" type="fixed">
+    <origin xyz="0.0605 0 0"/>
+    <parent link="down_arm_link"/>
+    <child link="servo3"/>
+  </joint>
+  <link name="servo4">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23735" ixy="-0.00045611" ixz="-0.0083689" iyy="0.23138" iyz="0.0096898" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.015 0 0"/>
+      <geometry>
+        <box size="0.030 0.030 0.025"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.030 0.030 0.025"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo3_joint" type="revolute">
+    <origin xyz="0.0145 0 0"/>
+    <parent link="servo3"/>
+    <child link="servo4"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="120" lower="-1.4" upper="1.4" velocity="2.0"/>
+  </joint>
+  <link name="servo4_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="9.0546"/>
+      <inertia ixx="0.23735" ixy="-0.00045611" ixz="-0.0083689" iyy="0.23138" iyz="0.0096898" izz="0.059867"/>
+    </inertial>
+    <visual>
+      <origin xyz="0.0375 0 0"/>
+      <geometry>
+        <box size="0.065 0.035 0.027"/>
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.065 0.035 0.027"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="servo4_joint" type="revolute">
+    <origin xyz="0.015 0 0"/>
+    <parent link="servo4"/>
+    <child link="servo4_link"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="120" lower="-1.4" upper="1.4" velocity="2.0"/>
+  </joint>
+  <!--
+  <link name="end_link"/>
+  <joint name="end_joint" type="fixed">
+    <origin
+      xyz="0 0 0.1118"
+      rpy="0 0 0" />
+    <parent
+      link="part5" />
+    <child
+      link="end_link" />
+  </joint>
+  -->
+</robot>
+

ros_code/src/robot_arm_description/model/arduino_robot_arm.urdf.xacro

@@ -0,0 +1,447 @@
+<?xml version="1.0"?>
+<robot name="arduino_robot_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find robot_arm_description)/model/materials.xacro" />
+  <xacro:include filename="$(find robot_arm_description)/model/arm_model.gazebo" />
+  <xacro:include filename="$(find robot_arm_description)/model/arm_transmission.xacro" />
+
+  <xacro:property name="PI" value="3.1415926535897931"/>
+  <xacro:property name="joint_limit" value="1.4"/>
+
+  <link name="world"/>
+  <joint name="world_base" type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 ${PI/2}" />
+    <parent link="world" />
+    <child  link="arm_base_link" />
+  </joint>
+
+  <link name="arm_base_link">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0.0325"/>
+      <geometry>
+        <cylinder length="0.065" radius="0.05" />
+      </geometry>
+      <material name="gray"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0.0325"
+        rpy="0 0 0" />
+      <geometry>
+        <cylinder length="0.065" radius="0.05" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="servo0">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="0.0096898"
+        iyy="0.23735"
+        iyz="0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.023 0.013 0.029" />
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.023 0.013 0.029" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo0_fix_joint"
+    type="fixed">
+    <origin
+      xyz="0 0 0.0505"
+      rpy="0 0 0" />
+    <parent link="arm_base_link" />
+    <child link="servo0" />
+    <axis xyz="0 0 0" />
+  </joint>
+
+  <link name="turn_panel">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <cylinder length="0.002" radius="0.04" />
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.002" radius="0.04" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo0_joint"
+    type="revolute">
+    <origin xyz="0 0 0.0145" />
+    <parent link="servo0" />
+    <child link="turn_panel" />
+    <axis xyz="0 0 1"/>
+    <limit
+      lower="${-1*joint_limit}"
+      upper="${joint_limit}"
+      effort="120"
+      velocity="2.0" />
+  </joint>
+
+  <link name="servo1">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.029 0.023 0.026" />
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.029 0.023 0.026" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo1_fix_joint"
+    type="fixed">
+    <origin xyz="0 0 0.014" />
+    <parent link="turn_panel" />
+    <child  link="servo1" />
+  </joint>
+
+  <link
+    name="up_arm_link">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass
+        value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0.04 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.08 0.035 0.027" />
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.08 0.035 0.027" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo1_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 -${PI/2} 0" />
+    <parent link="servo1" />
+    <child  link="up_arm_link" />
+    <axis xyz="0 1 0" />
+    <limit
+      lower="${-1*joint_limit}"
+      upper="${joint_limit}"
+      effort="120"
+      velocity="2.0" />
+  </joint>
+
+  <link
+    name="servo2">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.029 0.023 0.013" />
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.023 0.013" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo2_fix_joint"
+    type="fixed">
+    <origin xyz="0.0655 0 0"/>
+    <parent link="up_arm_link" />
+    <child  link="servo2" />
+  </joint>
+
+  <link name="down_arm_link">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23735"
+        ixy="-0.00045611"
+        ixz="-0.0083689"
+        iyy="0.23138"
+        iyz="0.0096898"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0375 0 0" />
+      <geometry>
+        <box size="0.075 0.035 0.027" />
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.075 0.035 0.027" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo2_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent link="servo2" />
+    <child  link="down_arm_link" />
+    <axis xyz="0 1 0" />
+    <limit
+      lower="${-1*joint_limit}"
+      upper="${joint_limit}"
+      effort="120"
+      velocity="2.0" />
+  </joint>
+
+  <link name="servo3">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23138"
+        ixy="0.00045611"
+        ixz="-0.0096898"
+        iyy="0.23735"
+        iyz="-0.0083689"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.029 0.013 0.023" />
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0"/>
+      <geometry>
+        <box size="0.029 0.013 0.023" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo3_fix_joint"
+    type="fixed">
+    <origin xyz="0.0605 0 0"/>
+    <parent link="down_arm_link" />
+    <child  link="servo3" />
+  </joint>
+
+  <link name="servo4">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23735"
+        ixy="-0.00045611"
+        ixz="-0.0083689"
+        iyy="0.23138"
+        iyz="0.0096898"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin xyz="0.015 0 0" />
+      <geometry>
+        <box size="0.030 0.030 0.025" />
+      </geometry>
+      <material name="blue"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.030 0.030 0.025" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo3_joint"
+    type="revolute">
+    <origin xyz="0.0145 0 0"/>
+    <parent link="servo3" />
+    <child  link="servo4" />
+    <axis xyz="1 0 0" />
+    <limit
+      lower="${-1*joint_limit}"
+      upper="${joint_limit}"
+      effort="120"
+      velocity="2.0" />
+  </joint>
+
+  <link name="servo4_link">
+    <inertial>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <mass value="9.0546" />
+      <inertia
+        ixx="0.23735"
+        ixy="-0.00045611"
+        ixz="-0.0083689"
+        iyy="0.23138"
+        iyz="0.0096898"
+        izz="0.059867" />
+    </inertial>
+    <visual>
+      <origin xyz="0.0375 0 0" />
+      <geometry>
+        <box size="0.065 0.035 0.027" />
+      </geometry>
+      <material name="yellow"/>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <box size="0.065 0.035 0.027" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="servo4_joint"
+    type="revolute">
+    <origin xyz="0.015 0 0" />
+    <parent link="servo4" />
+    <child  link="servo4_link" />
+    <axis xyz="0 1 0" />
+    <limit
+      lower="${-1*joint_limit}"
+      upper="${joint_limit}"
+      effort="120"
+      velocity="2.0" />
+  </joint>
+
+  <!--
+  <link name="end_link"/>
+  <joint name="end_joint" type="fixed">
+    <origin
+      xyz="0 0 0.1118"
+      rpy="0 0 0" />
+    <parent
+      link="part5" />
+    <child
+      link="end_link" />
+  </joint>
+  -->
+</robot>
+

ros_code/src/robot_arm_description/model/arm_model.gazebo

@@ -0,0 +1,121 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ros_control plugin -->
+  <gazebo>
+    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+      
+    </plugin>
+  </gazebo>
+  
+  <gazebo reference="cute_base_link">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part1">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Yellow</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part2">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part3">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Yellow</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part4">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part5">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Yellow</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part6">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="part7">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="claw1">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+  <gazebo reference="claw2">
+    <visual name='visual'>
+     <material>
+       <script>
+         <uri>file://media/materials/scripts/gazebo.material</uri>
+         <name>Gazebo/Grey</name>
+       </script>
+     </material>
+    </visual>
+  </gazebo>
+  
+</robot>

ros_code/src/robot_arm_description/model/arm_transmission.xacro

@@ -0,0 +1,83 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <transmission name="joint1_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint1">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint1_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint2_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint2">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint2_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint3_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint3">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint3_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint4_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint4">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint4_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint5_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint5">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint5_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint6_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint6">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint6_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="joint7_trans">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="joint7">
+        <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="joint7_motor">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+    <transmission name="claw_trans_1">
+      <type>transmission_interface/SimpleTransmission</type>
+      <joint name="claw">
+        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      </joint>
+      <actuator name="claw_motor_1">
+        <mechanicalReduction>1</mechanicalReduction>
+      </actuator>
+    </transmission>
+
+</robot>

ros_code/src/robot_arm_description/model/make_URDF.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+rosrun xacro xacro.py --inorder arduino_robot_arm.urdf.xacro > arduino_robot_arm.urdf
+
+check_urdf arduino_robot_arm.urdf
+
+exit 0

ros_code/src/robot_arm_description/model/materials.xacro

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <material name="black">
+    <color rgba="0.0 0.0 0.0 1.0"/>
+  </material>
+
+  <material name="blue">
+    <color rgba="0.0 0.0 0.8 1.0"/>
+  </material>
+
+  <material name="green">
+    <color rgba="0.0 0.8 0.0 1.0"/>
+  </material>
+
+  <material name="grey">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+
+  <material name="orange">
+    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
+  </material>
+
+  <material name="brown">
+    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
+  </material>
+
+  <material name="red">
+    <color rgba="0.8 0.0 0.0 1.0"/>
+  </material>
+
+  <material name="white">
+    <color rgba="1.0 1.0 1.0 1.0"/>
+  </material>
+  
+  <material name="yellow">
+    <color rgba="${202/255} ${209/255} 0.0 1.0" />
+  </material>
+  
+  <material name="gray">
+    <color rgba="${192/255} ${192/255} ${192/255} 1.0" />
+  </material>
+
+</robot>

ros_code/src/robot_arm_description/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_arm_description</name>
+  <version>0.0.0</version>
+  <description>The robot_arm_description package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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_arm_description</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>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/robot_arm_moveit_config/.setup_assistant

@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: robot_arm_description
+    relative_path: model/arduino_robot_arm.urdf
+    use_jade_xacro: true
+  SRDF:
+    relative_path: config/arduino_robot_arm.srdf
+  CONFIG:
+    author_name: corvin
+    author_email: corvin_zhang@corvin.cn
+    generated_timestamp: 1545968267

ros_code/src/robot_arm_moveit_config/CMakeLists.txt

@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_arm_moveit_config)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

ros_code/src/robot_arm_moveit_config/config/arduino_robot_arm.srdf

@@ -0,0 +1,74 @@
+<?xml version="1.0" ?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="arduino_robot_arm">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="robot_arm">
+        <chain base_link="arm_base_link" tip_link="servo4_link" />
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="resting" group="robot_arm">
+        <joint name="servo0_joint" value="0" />
+        <joint name="servo1_joint" value="-1" />
+        <joint name="servo2_joint" value="1.4" />
+        <joint name="servo3_joint" value="0" />
+        <joint name="servo4_joint" value="1.4" />
+    </group_state>
+    <group_state name="straight_up" group="robot_arm">
+        <joint name="servo0_joint" value="0" />
+        <joint name="servo1_joint" value="0" />
+        <joint name="servo2_joint" value="0" />
+        <joint name="servo3_joint" value="0" />
+        <joint name="servo4_joint" value="0" />
+    </group_state>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="arm_base_link" link2="servo0" reason="Adjacent" />
+    <disable_collisions link1="arm_base_link" link2="servo1" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="servo2" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="servo3" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="servo4" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="servo4_link" reason="Never" />
+    <disable_collisions link1="arm_base_link" link2="turn_panel" reason="Default" />
+    <disable_collisions link1="arm_base_link" link2="up_arm_link" reason="Default" />
+    <disable_collisions link1="down_arm_link" link2="servo0" reason="Never" />
+    <disable_collisions link1="down_arm_link" link2="servo1" reason="Never" />
+    <disable_collisions link1="down_arm_link" link2="servo2" reason="Adjacent" />
+    <disable_collisions link1="down_arm_link" link2="servo3" reason="Adjacent" />
+    <disable_collisions link1="down_arm_link" link2="servo4" reason="Never" />
+    <disable_collisions link1="down_arm_link" link2="servo4_link" reason="Never" />
+    <disable_collisions link1="down_arm_link" link2="up_arm_link" reason="Default" />
+    <disable_collisions link1="servo0" link2="servo1" reason="Never" />
+    <disable_collisions link1="servo0" link2="servo2" reason="Never" />
+    <disable_collisions link1="servo0" link2="servo3" reason="Never" />
+    <disable_collisions link1="servo0" link2="servo4" reason="Never" />
+    <disable_collisions link1="servo0" link2="servo4_link" reason="Never" />
+    <disable_collisions link1="servo0" link2="turn_panel" reason="Adjacent" />
+    <disable_collisions link1="servo0" link2="up_arm_link" reason="Default" />
+    <disable_collisions link1="servo1" link2="servo2" reason="Never" />
+    <disable_collisions link1="servo1" link2="servo3" reason="Never" />
+    <disable_collisions link1="servo1" link2="servo4" reason="Never" />
+    <disable_collisions link1="servo1" link2="servo4_link" reason="Never" />
+    <disable_collisions link1="servo1" link2="turn_panel" reason="Adjacent" />
+    <disable_collisions link1="servo1" link2="up_arm_link" reason="Adjacent" />
+    <disable_collisions link1="servo2" link2="servo3" reason="Never" />
+    <disable_collisions link1="servo2" link2="servo4" reason="Never" />
+    <disable_collisions link1="servo2" link2="servo4_link" reason="Never" />
+    <disable_collisions link1="servo2" link2="turn_panel" reason="Never" />
+    <disable_collisions link1="servo2" link2="up_arm_link" reason="Adjacent" />
+    <disable_collisions link1="servo3" link2="servo4" reason="Adjacent" />
+    <disable_collisions link1="servo3" link2="servo4_link" reason="Default" />
+    <disable_collisions link1="servo3" link2="turn_panel" reason="Never" />
+    <disable_collisions link1="servo3" link2="up_arm_link" reason="Never" />
+    <disable_collisions link1="servo4" link2="servo4_link" reason="Adjacent" />
+    <disable_collisions link1="servo4" link2="turn_panel" reason="Never" />
+    <disable_collisions link1="servo4" link2="up_arm_link" reason="Never" />
+    <disable_collisions link1="servo4_link" link2="turn_panel" reason="Never" />
+    <disable_collisions link1="servo4_link" link2="up_arm_link" reason="Never" />
+    <disable_collisions link1="turn_panel" link2="up_arm_link" reason="Default" />
+</robot>

ros_code/src/robot_arm_moveit_config/config/controllers.yaml

@@ -0,0 +1,11 @@
+controller_list:
+  - name: robot_arm_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - servo0_joint
+      - servo1_joint
+      - servo2_joint
+      - servo3_joint
+      - servo4_joint

ros_code/src/robot_arm_moveit_config/config/fake_controllers.yaml

@@ -0,0 +1,8 @@
+controller_list:
+  - name: fake_robot_arm_controller
+    joints:
+      - servo0_joint
+      - servo1_joint
+      - servo2_joint
+      - servo3_joint
+      - servo4_joint

ros_code/src/robot_arm_moveit_config/config/joint_limits.yaml

@@ -0,0 +1,29 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  servo0_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  servo1_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  servo2_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  servo3_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0
+  servo4_joint:
+    has_velocity_limits: true
+    max_velocity: 2
+    has_acceleration_limits: false
+    max_acceleration: 0

ros_code/src/robot_arm_moveit_config/config/kinematics.yaml

@@ -0,0 +1,5 @@
+robot_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
+  kinematics_solver_attempts: 3

ros_code/src/robot_arm_moveit_config/config/ompl_planning.yaml

@@ -0,0 +1,67 @@
+planner_configs:
+  SBLkConfigDefault:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ESTkConfigDefault:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECEkConfigDefault:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECEkConfigDefault:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECEkConfigDefault:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05 
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRTkConfigDefault:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnectkConfigDefault:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstarkConfigDefault:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRTkConfigDefault:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRMkConfigDefault:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstarkConfigDefault:
+    type: geometric::PRMstar
+robot_arm:
+  planner_configs:
+    - SBLkConfigDefault
+    - ESTkConfigDefault
+    - LBKPIECEkConfigDefault
+    - BKPIECEkConfigDefault
+    - KPIECEkConfigDefault
+    - RRTkConfigDefault
+    - RRTConnectkConfigDefault
+    - RRTstarkConfigDefault
+    - TRRTkConfigDefault
+    - PRMkConfigDefault
+    - PRMstarkConfigDefault

ros_code/src/robot_arm_moveit_config/config/table_scene.scene

@@ -0,0 +1,2 @@
+(noname)++
+.

ros_code/src/robot_arm_moveit_config/launch/arduino_robot_arm_moveit_controller_manager.launch.xml

@@ -0,0 +1,15 @@
+<!--
+  Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+  Author: corvin
+  Description: 该启动文件用于启动控制器管理器.
+  History:
+    20190104: Initial this file.
+-->
+<launch>
+    <!-- set the param that trajectory_execution_manager needs to find the controller plugin -->
+    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+
+    <!-- load robot arm controller list yaml file -->
+    <rosparam file="$(find robot_arm_moveit_config)/config/controllers.yaml" />
+</launch>

ros_code/src/robot_arm_moveit_config/launch/arduino_robot_arm_moveit_sensor_manager.launch.xml

@@ -0,0 +1,3 @@
+<launch>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/default_warehouse_db.launch

@@ -0,0 +1,15 @@
+<launch>
+
+  <arg name="reset" default="false"/>
+  <!-- If not specified, we'll use a default database location -->
+  <arg name="moveit_warehouse_database_path" default="$(find robot_arm_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- Launch the warehouse with the configured database location -->
+  <include file="$(find robot_arm_moveit_config)/launch/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
+  </include>
+
+  <!-- If we want to reset the database, run this node -->
+  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/demo.launch

@@ -0,0 +1,56 @@
+<launch>
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find robot_arm_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!--
+  By default, hide joint_state_publisher's GUI
+
+  MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
+  The latter one maintains and publishes the current joint configuration of the simulated robot.
+  It also provides a GUI to move the simulated robot around "manually".
+  This corresponds to moving around the real robot without the use of MoveIt.
+  -->
+  <arg name="use_gui" default="false" />
+
+  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
+  <include file="$(find robot_arm_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->
+
+  <!-- We do not have a robot connected, so publish fake joint states -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="$(arg use_gui)"/>
+    <rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
+  </node>
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find robot_arm_moveit_config)/launch/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="true"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find robot_arm_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find robot_arm_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/fake_moveit_controller_manager.launch.xml

@@ -0,0 +1,9 @@
+<launch>
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
+
+  <!-- The rest of the params are specific to this plugin -->
+  <rosparam file="$(find robot_arm_moveit_config)/config/fake_controllers.yaml"/>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/joystick_control.launch

@@ -0,0 +1,17 @@
+<launch>
+  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
+
+  <arg name="dev" default="/dev/input/js0" />
+
+  <!-- Launch joy node -->
+  <node pkg="joy" type="joy_node" name="joy">
+    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
+    <param name="deadzone" value="0.2" />
+    <param name="autorepeat_rate" value="40" />
+    <param name="coalesce_interval" value="0.025" />
+  </node>
+
+  <!-- Launch python interface -->
+  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
+        
+</launch>

ros_code/src/robot_arm_moveit_config/launch/move_group.launch

@@ -0,0 +1,74 @@
+<launch>
+  <include file="$(find robot_arm_moveit_config)/launch/planning_context.launch" />
+
+  <!-- GDB Debug Option -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix"
+	   value="gdb -x $(find robot_arm_moveit_config)/launch/gdb_settings.gdb --ex run --args" />
+
+  <!-- Verbose Mode Option -->
+  <arg name="info" default="$(arg debug)" />
+  <arg unless="$(arg info)" name="command_args" value="" />
+  <arg     if="$(arg info)" name="command_args" value="--debug" />
+
+  <!-- move_group settings -->
+  <arg name="allow_trajectory_execution" default="true"/>
+  <arg name="fake_execution" default="false"/>
+  <arg name="max_safe_path_cost" default="1"/>
+  <arg name="jiggle_fraction" default="0.05" />
+  <arg name="publish_monitored_planning_scene" default="true"/>
+
+  <!-- Planning Functionality -->
+  <include ns="move_group" file="$(find robot_arm_moveit_config)/launch/planning_pipeline.launch.xml">
+    <arg name="pipeline" value="ompl" />
+  </include>
+
+  <!-- Trajectory Execution Functionality -->
+  <include ns="move_group" file="$(find robot_arm_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_manage_controllers" value="true" />
+    <arg name="moveit_controller_manager" value="arduino_robot_arm" unless="$(arg fake_execution)"/>
+    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
+  </include>
+
+  <!-- Sensors Functionality -->
+  <include ns="move_group" file="$(find robot_arm_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_sensor_manager" value="arduino_robot_arm" />
+  </include>
+
+  <!-- Start the actual move_group node/action server -->
+  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
+    <!-- Set the display variable, in case OpenGL code is used internally -->
+    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
+
+    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+    <param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
+    <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+    <!-- corvin add -->
+    <param name="trajectory_execution/allowed_execution_duration_scaling" value="5" />
+    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5" />
+
+    <!-- load these non-default MoveGroup capabilities -->
+    <!--
+    <param name="capabilities" value="
+                  a_package/AwsomeMotionPlanningCapability
+                  another_package/GraspPlanningPipeline
+                  " />
+    -->
+
+    <!-- inhibit these default MoveGroup capabilities -->
+    <!--
+    <param name="disable_capabilities" value="
+                  move_group/MoveGroupKinematicsService
+                  move_group/ClearOctomapService
+                  " />
+    -->
+
+    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
+    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
+  </node>
+</launch>

ros_code/src/robot_arm_moveit_config/launch/moveit.rviz

@@ -0,0 +1,320 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+        - /TF1/Frames1
+      Splitter Ratio: 0.483631
+    Tree Height: 338
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Goal_Tolerance: 0
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Constraint_Aware_IK: true
+      MoveIt_Warehouse_Host: 127.0.0.1
+      MoveIt_Warehouse_Port: 33829
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          arm_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          down_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          turn_panel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          up_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Loop Animation: false
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: true
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.08
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: robot_arm
+        Query Goal State: true
+        Query Start State: true
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: /move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.2
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          arm_base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          down_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo0:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          servo4_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          turn_panel:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          up_arm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+        Robot Alpha: 0.5
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        arm_base_link:
+          Value: true
+        down_arm_link:
+          Value: true
+        servo0:
+          Value: true
+        servo1:
+          Value: true
+        servo2:
+          Value: true
+        servo3:
+          Value: true
+        servo4:
+          Value: true
+        servo4_link:
+          Value: true
+        turn_panel:
+          Value: true
+        up_arm_link:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.15
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          arm_base_link:
+            servo0:
+              turn_panel:
+                servo1:
+                  up_arm_link:
+                    servo2:
+                      down_arm_link:
+                        servo3:
+                          servo4:
+                            servo4_link:
+                              {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 0.899889
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.287438
+        Y: -0.0412742
+        Z: 2.4587e-07
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.070202
+      Target Frame: world
+      Value: XYOrbit (rviz)
+      Yaw: 0.0867624
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1028
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000002a2000003befc0200000007fb000000100044006900730070006c0061007900730100000028000001e1000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001d7000001cc00ffffff0000047c000003be00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1828
+  X: 29
+  Y: 14

ros_code/src/robot_arm_moveit_config/launch/moveit_rviz.launch

@@ -0,0 +1,16 @@
+<launch>
+
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="config" default="false" />
+  <arg unless="$(arg config)" name="command_args" value="" />
+  <arg     if="$(arg config)" name="command_args" value="-d $(find robot_arm_moveit_config)/launch/moveit.rviz" />
+  
+  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+	args="$(arg command_args)" output="screen">
+    <rosparam command="load" file="$(find robot_arm_moveit_config)/config/kinematics.yaml"/>
+  </node>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/ompl_planning_pipeline.launch.xml

@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- OMPL Plugin for MoveIt! -->
+  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
+
+  <!-- The request adapters (plugins) used when planning with OMPL. 
+       ORDER MATTERS -->
+  <arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
+				       default_planner_request_adapters/FixWorkspaceBounds
+				       default_planner_request_adapters/FixStartStateBounds
+				       default_planner_request_adapters/FixStartStateCollision
+				       default_planner_request_adapters/FixStartStatePathConstraints" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+
+  <rosparam command="load" file="$(find robot_arm_moveit_config)/config/ompl_planning.yaml"/>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/planning_context.launch

@@ -0,0 +1,23 @@
+<launch>
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="false"/>
+
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
+
+  <!-- Load universal robot description format (URDF) -->
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find robot_arm_description)/model/arduino_robot_arm.urdf"/>
+
+  <!-- The semantic description that corresponds to the URDF -->
+  <param name="$(arg robot_description)_semantic" textfile="$(find robot_arm_moveit_config)/config/arduino_robot_arm.srdf" />
+  
+  <!-- Load updated joint limits (override information from URDF) -->
+  <group ns="$(arg robot_description)_planning">
+    <rosparam command="load" file="$(find robot_arm_moveit_config)/config/joint_limits.yaml"/>
+  </group>
+
+  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+  <group ns="$(arg robot_description)_kinematics">
+    <rosparam command="load" file="$(find robot_arm_moveit_config)/config/kinematics.yaml"/>
+  </group>
+</launch>

ros_code/src/robot_arm_moveit_config/launch/planning_pipeline.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+
+  <!-- This file makes it easy to include different planning pipelines; 
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->  
+
+  <arg name="pipeline" default="ompl" />
+
+  <include file="$(find robot_arm_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" />
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/robot_arm.launch

@@ -0,0 +1,48 @@
+<launch>
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find robot_arm_moveit_config)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, hide joint_state_publisher's GUI -->
+  <arg name="use_gui" default="false" />
+
+  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
+  <include file="$(find robot_arm_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- If needed, broadcast static tf for robot root -->
+
+  <!-- We connected arduino robot arm, so publish joint states -->
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="/use_gui" value="$(arg use_gui)"/>
+    <rosparam param="/source_list">[/move_group/controller_joint_states]</rosparam>
+  </node>
+
+  <!-- Given the published joint states, publish tf for the robot links -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(find robot_arm_moveit_config)/launch/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="fake_execution" value="false"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(find robot_arm_moveit_config)/launch/moveit_rviz.launch">
+    <arg name="config" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(find robot_arm_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+</launch>

ros_code/src/robot_arm_moveit_config/launch/run_benchmark_ompl.launch

@@ -0,0 +1,22 @@
+<launch>
+
+  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
+  <arg name="cfg" />
+
+  <!-- Load URDF -->
+  <include file="$(find robot_arm_moveit_config)/launch/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Start the database -->
+  <include file="$(find robot_arm_moveit_config)/launch/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
+  </include>  
+
+  <!-- Start Benchmark Executable -->
+  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
+    <rosparam command="load" file="$(find robot_arm_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find robot_arm_moveit_config)/config/ompl_planning.yaml"/>
+  </node>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/sensor_manager.launch.xml

@@ -0,0 +1,14 @@
+<launch>
+
+  <!-- This file makes it easy to include the settings for sensor managers -->  
+
+  <!-- Params for the octomap monitor -->
+  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
+
+  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
+  <arg name="moveit_sensor_manager" default="arduino_robot_arm" />
+  <include file="$(find robot_arm_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
+  
+</launch>

ros_code/src/robot_arm_moveit_config/launch/setup_assistant.launch

@@ -0,0 +1,15 @@
+<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
+<launch>
+
+  <!-- Debug Info -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <!-- Run -->
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant" 
+	args="--config_pkg=robot_arm_moveit_config"
+	launch-prefix="$(arg launch_prefix)"
+        output="screen" />
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/trajectory_execution.launch.xml

@@ -0,0 +1,20 @@
+<launch>
+  <!-- This file makes it easy to include the settings for trajectory execution  -->
+
+  <!-- Flag indicating whether MoveIt! is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="3"/> <!-- default 1.2 -->
+
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
+
+  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+  <param name="trajectory_execution/allowed_start_tolerance" value="0.02"/> <!-- default 0.01 -->
+
+  <!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
+  <arg name="moveit_controller_manager" default="arduino_robot_arm" />
+  <include file="$(find robot_arm_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
+</launch>

ros_code/src/robot_arm_moveit_config/launch/warehouse.launch

@@ -0,0 +1,15 @@
+<launch>
+  
+  <!-- The path to the database must be specified -->
+  <arg name="moveit_warehouse_database_path" />
+
+  <!-- Load warehouse parameters -->  
+  <include file="$(find robot_arm_moveit_config)/launch/warehouse_settings.launch.xml" />
+
+  <!-- Run the DB server -->
+  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros">
+    <param name="overwrite" value="false"/>
+    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
+  </node>
+
+</launch>

ros_code/src/robot_arm_moveit_config/launch/warehouse_settings.launch.xml

@@ -0,0 +1,15 @@
+<launch>
+  <!-- Set the parameters for the warehouse and run the mongodb server. -->
+
+  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->  
+  <arg name="moveit_warehouse_port" default="33829" /> 
+
+  <!-- The default DB host for moveit -->
+  <arg name="moveit_warehouse_host" default="localhost" /> 
+  
+  <!-- Set parameters for the warehouse -->
+  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
+  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
+  <param name="warehouse_exec" value="mongod" />
+
+</launch>

ros_code/src/robot_arm_moveit_config/package.xml

@@ -0,0 +1,31 @@
+<package>
+
+  <name>robot_arm_moveit_config</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the arduino_robot_arm with the MoveIt! Motion Planning Framework
+  </description>
+  <author email="corvin_zhang@corvin.cn">corvin</author>
+  <maintainer email="corvin_zhang@corvin.cn">corvin</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
+  <run_depend>moveit_kinematics</run_depend>
+  <run_depend>moveit_planners_ompl</run_depend>
+  <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>xacro</run_depend>
+  <build_depend>robot_arm_description</build_depend>
+  <run_depend>robot_arm_description</run_depend>
+
+
+</package>

ros_code/src/robot_arm_moveit_test/CMakeLists.txt

@@ -0,0 +1,211 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_arm_moveit_test)
+
+## 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
+  cmake_modules
+  interactive_markers
+  moveit_core
+  moveit_ros_perception
+  moveit_ros_planning_interface
+  pluginlib
+  roscpp
+  rospy
+  std_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
+# )
+
+################################################
+## 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_arm_moveit_test
+#  CATKIN_DEPENDS cmake_modules interactive_markers moveit_core moveit_ros_perception moveit_ros_planning_interface pluginlib roscpp rospy std_msgs
+#  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_arm_moveit_test.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(random_joint_node src/random_joint.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(random_joint_node robot_arm_moveit_test_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+#target_link_libraries(random_joint_node
+#  ${catkin_LIBRARIES}
+#)
+
+#add_executable(custom_joint_node src/custom_joint.cpp)
+#add_dependencies(custom_joint_node robot_arm_moveit_test_generate_messages_cpp)
+#target_link_libraries(custom_joint_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
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_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_arm_moveit_test.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)

ros_code/src/robot_arm_moveit_test/package.xml

@@ -0,0 +1,86 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>robot_arm_moveit_test</name>
+  <version>0.0.0</version>
+  <description>The robot_arm_moveit_test package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="corvin@todo.todo">corvin</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_arm_moveit_test</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>cmake_modules</build_depend>
+  <build_depend>interactive_markers</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_ros_perception</build_depend>
+  <build_depend>moveit_ros_planning_interface</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>cmake_modules</build_export_depend>
+  <build_export_depend>interactive_markers</build_export_depend>
+  <build_export_depend>moveit_core</build_export_depend>
+  <build_export_depend>moveit_ros_perception</build_export_depend>
+  <build_export_depend>moveit_ros_planning_interface</build_export_depend>
+  <build_export_depend>pluginlib</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>cmake_modules</exec_depend>
+  <exec_depend>interactive_markers</exec_depend>
+  <exec_depend>moveit_core</exec_depend>
+  <exec_depend>moveit_ros_perception</exec_depend>
+  <exec_depend>moveit_ros_planning_interface</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ros_code/src/robot_arm_moveit_test/src/moveit_fk_demo.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+
+import rospy, sys
+import moveit_commander
+
+class MoveItDemo:
+    def __init__(self):
+        #Initialize the move_group API
+        moveit_commander.roscpp_initialize(sys.argv)
+
+        #Initialize the ROS node
+        rospy.init_node('moveit_fk_demo', anonymous=True)
+
+        # connect to the robot_arm move group
+        robot_arm = moveit_commander.MoveGroupCommander('robot_arm')
+
+        #start the arm in the "resting"
+        robot_arm.set_named_target("resting")
+
+        #plan a trajectory to the goal configuration
+        traj=robot_arm.plan()
+
+        #Execute the planned trajectory
+        robot_arm.execute(traj)
+        rospy.loginfo("After move arm to resting pose !!!")
+        rospy.sleep(2)
+
+        #set target joint values for the arm
+        joint_positions = [-0.08, 1.2, 0.02, 0.08, -1.2]
+
+        #Set the arm's goal configuration to the joint position
+        robot_arm.set_joint_value_target(joint_positions)
+
+        #Plan and execute a trajectory to the goal configuration
+        robot_arm.go()
+        rospy.loginfo("After move arm to custom joint pose !!!")
+        rospy.sleep(2)
+
+        #Save this configuration for later
+        robot_arm.remember_joint_values('saved_config', joint_positions)
+
+        #Set the arm target to the "straight_up
+        robot_arm.set_named_target("straight_up")
+        robot_arm.go()
+        rospy.loginfo("After move arm to straight_up pose !!!")
+        rospy.sleep(2)
+
+        #set the goal configuration to the named configuration saved
+        robot_arm.set_named_target('saved_config')
+        robot_arm.go()
+        rospy.loginfo("After move arm to saved_config pose !!!")
+        rospy.sleep(2)
+
+        #Return the arm to default pose
+        robot_arm.set_named_target("resting")
+        robot_arm.go()
+        rospy.loginfo("After move arm to resting pose !!!")
+        rospy.sleep(2)
+
+        #Cleanly shutdown moveIt!
+        moveit_commander.roscpp_shutdown()
+
+        # Exit the script
+        moveit_commander.os._exit(0)
+
+if __name__ == "__main__":
+    try:
+        MoveItDemo()
+    except rospy.ROSInterruptException():
+        pass

scripts/clone_code.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+# Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+# Author: corvin
+# Description: 该脚本主要用于git clone下载代码,由于默认的master分支代码为空,
+#    只有切换到正常的indigo或kinetic分支才能看到完整代码.该脚本下载的代码仓库,
+#    不具有提交代码的权限,如果要想提交代码，需要登录gerrit帐号,然后在里面使用
+#    ssh方式来下载代码,这样在gerrit上配置好公钥就可以提交代码到gerrit进行审核、
+#    合并代码了.
+# History:
+#    20190104: Initial this bash file.
+
+git clone -b kinetic-devel http://corvin.cn:8081/gerrit/arduino_robot_arm
+
+exit 0

scripts/setup_env.sh

@@ -0,0 +1,40 @@
+#!/bin/bash
+
+# Copyright: 2016-2019 ROS小课堂 www.corvin.cn
+# Author: corvin
+# Description:该脚本是为了编译工作去软件包,并配置环境变量到.bahsrc.
+# History:
+#   20181226:Initial this bash file.
+#   20190122:新增在运行前需要安装的一些必要软件包.
+
+red="\e[31m"
+normal="\e[0m"
+CURRENT_PATH=$(dirname $(readlink -f "$0"))
+WS_PATH=${CURRENT_PATH%scripts}
+
+# first install necessary packages
+sudo apt update
+while [ $? -ne 0 ]
+do
+    echo -e "{red}Can't update source list, will retry...{noraml}\n"
+    sleep 3
+    sudo apt update
+done
+sudo apt -y upgrade
+
+sudo apt install -y ros-kinetic-moveit-msgs ros-kinetic-moveit-core \
+    ros-kinetic-moveit-ros-perception ros-kinetic-moveit-ros-planning-interface \
+    ros-kinetic-moveit
+
+sudo apt-get -y autoremove
+
+# second catkin_make worksapce
+cd $WS_PATH/ros_code/
+catkin_make
+
+# third set workspace env to .bashrc
+source devel/setup.bash
+echo "#config arduino_robot_arm env" >>~/.bashrc
+echo "source ${WS_PATH}ros_code/devel/setup.bash" >> ~/.bashrc
+
+exit 0