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

ROSCode/.catkin_workspace

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

ROSCode/src/CMakeLists.txt

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

ROSCode/src/android_app_communication/CMakeLists.txt

@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(android_app_communication)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+)
+
+## 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 run_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 run_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
+  androidLogMsg.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 run_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 you 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 android_app_communication
+#  CATKIN_DEPENDS 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}/android_app_communication.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/android_app_communication_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_android_app_communication.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)

ROSCode/src/android_app_communication/msg/androidLogMsg.msg

@@ -0,0 +1 @@
+string message

ROSCode/src/android_app_communication/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package>
+  <name>android_app_communication</name>
+  <version>0.0.0</version>
+  <description>The android_app_communication 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/android_app_communication</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 build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ROSCode/src/bringup/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(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 COMPONENTS
+  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 run_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 run_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 run_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 you 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 bringup
+#  CATKIN_DEPENDS 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}/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/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_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)

ROSCode/src/bringup/launch/minimal.launch

@@ -0,0 +1,75 @@
+<launch>
+    <arg name="androidLog_topic" default="/android_communication/robotToAppLog" />    
+
+    <!-- startup robot description -->
+    <include file="$(find description)/launch/display.launch"/>
+
+    <!-- startup mobilebase arduino launch -->
+    <include file="$(find ros_arduino_python)/launch/arduino.launch" />
+
+    <!-- startup IMU node launch-->
+    <include file="$(find razor_imu_9dof)/launch/razor-pub.launch" />
+
+    <!-- startup robot_pose_ekf node -->
+    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
+        <param name="output_frame" value="odom_combined"/>
+        <param name="base_footprint_frame" value="base_footprint"/>
+        <param name="freq" value="20.0"/>
+        <param name="sensor_timeout" value="1.0"/>  
+        <param name="odom_used" value="true"/>
+        <param name="imu_used" value="true"/>
+        <param name="vo_used" value="false"/>
+        <param name="debug" value="false"/>
+        <remap from="imu_data" to="imu" />
+    </node>
+    
+    <!-- transform between /odom and /odom_combined -->
+    <node pkg="tf" type="static_transform_publisher" name="odom_combined_broadcaster" args="0 0 0 0 0 0 /odom /odom_combined 100" />
+
+    <!-- startup convert /odom_combined to /odom_ekf can view in rviz-->
+    <include file="$(find bringup)/launch/odom_ekf.launch"/>
+
+    <!-- startup protect motor node -->
+    <node pkg="protect_motors" type="protect_motors_node" name="protect_motor_node" output="screen">
+      <param name="max_current" value="1.9"/>
+      <param name="motor_power_pin" value="52"/>
+      <param name="alarm_power_pin" value="51"/>
+      <param name="delay_time" value="5"/>
+      <param name="current_topic" value="/mobilebase_arduino/sensor/motors_current"/>
+      <param name="alarm_control_service" value="/mobilebase_arduino/digital_write"/>
+    </node>
+
+    <!-- startup rplidar node -->
+    <include file="$(find rplidar_ros)/launch/rplidar.launch" />
+
+    <!-- startup move_base node -->
+    <include file="$(find navigation)/launch/move_base.launch" />
+
+    <!-- startup voice system node -->
+    <node pkg="voice_system" type="xf_tts_node" name="xf_tts_node" output="screen" >
+      <param name="app_id" type="String" value="57207923" />
+      <!--xiaowanzi, xiaoxin, vinn, xiaofeng, xiaoqi etc.-->
+      <param name="speaker_name" type="String" value="xiaowanzi" />
+    </node>
+
+    <!-- startup usb_cam project -->
+    <include file="$(find usb_cam)/launch/usb_cam-test.launch"/>
+
+    <!-- startup head arduino node-->
+    <include file="$(find head_arduino_python)/launch/head_arduino.launch" />
+    
+    <!-- startup check_sensor_status -->
+    <include file="$(find check_sensor_status)/launch/check_sensor.launch" >
+      <arg name="androidLog_topic" value="$(arg androidLog_topic)" />
+    </include>
+
+    <!-- startup camera monitor -->
+    <!--<include file="$(find camera_monitor)/launch/camera_monitor.launch" />-->
+  
+    <!-- projector launch -->
+    <include file="$(find projector_control)/launch/projector.launch" >
+      <arg name="androidLog_topic" value="$(arg androidLog_topic)" />
+    </include>
+
+</launch>
+

ROSCode/src/bringup/launch/odom_ekf.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
+    <remap from="output" to="/odom_ekf"/>
+  </node>
+</launch>

ROSCode/src/bringup/launch/odom_ekf.py

@@ -0,0 +1,63 @@
+#!/usr/bin/env python
+
+""" odom_ekf.py - Version 1.1 2013-12-20
+
+    Republish the /robot_pose_ekf/odom_combined topic which is of type 
+    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
+    type nav_msgs/Odometry so we can view it in RViz.
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.5
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+      
+"""
+
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
+
+class OdomEKF():
+    def __init__(self):
+        # Give the node a name
+        rospy.init_node('odom_ekf', anonymous=False)
+
+        # Publisher of type nav_msgs/Odometry
+        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=10)
+        
+        # Wait for the /odom_combined topic to become available
+        rospy.wait_for_message('input', PoseWithCovarianceStamped)
+        
+        # Subscribe to the /odom_combined topic
+        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
+        
+        rospy.loginfo("Publishing combined odometry on /odom_ekf")
+        
+    def pub_ekf_odom(self, msg):
+        odom = Odometry()
+        odom.header = msg.header
+        odom.header.frame_id = '/odom'
+        odom.child_frame_id = 'base_footprint'
+        odom.pose = msg.pose
+        
+        self.ekf_pub.publish(odom)
+        
+if __name__ == '__main__':
+    try:
+        OdomEKF()
+        rospy.spin()
+    except:
+        pass
+        
+
+        

ROSCode/src/bringup/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>bringup</name>
+  <version>0.0.0</version>
+  <description>The 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/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 build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ROSCode/src/bringup/startup/mobilebase-restart

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+log_file=/tmp/mobilebase-upstart.log
+DATE=`date`
+echo "$DATE: mobilebase-stop" >> $log_file
+
+/usr/sbin/mobilebase-stop
+sleep 2
+/usr/sbin/mobilebase-start 

ROSCode/src/bringup/startup/mobilebase-start

@@ -0,0 +1,21 @@
+#!/bin/bash
+
+log_file=/tmp/mobilebase-upstart.log
+DATE=`date`
+
+source /opt/ros/indigo/setup.bash
+source /home/pi/corvinRobot_YOYO/ROSCode/devel/setup.bash
+
+interface=wlan0
+echo "$DATE: mobilebase-start on interface $interface" >> $log_file
+
+export ROS_IP=`ifconfig $interface | grep -o 'inet addr:[^ ]*' | cut -d: -f2`
+
+echo "$DATE: mobilebase-start setting ROS_IP=$ROS_IP" >> $log_file
+
+if [ "$ROS_IP" = "" ]; then
+    echo "$DATE: mobilebase-start can't run with empty ROS_IP." >> $log_file
+    exit 1
+fi
+
+roslaunch bringup minimal.launch

ROSCode/src/bringup/startup/mobilebase-stop

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+log_file=/tmp/mobilebase-upstart.log
+DATE=`date`
+echo "$DATE: mobilebase-stop" >> $log_file
+
+source /opt/ros/indigo/setup.bash
+source /home/pi/corvinRobot_YOYO/ROSCode/devel/setup.bash
+
+rosservice call /stop_motor
+
+for i in $( rosnode list ); do
+    rosnode kill $i;
+done
+
+killall roslaunch

ROSCode/src/bringup/startup/mobilebase.service

@@ -0,0 +1,12 @@
+[Unit]
+Description=mobilebase startup
+
+[Service]
+Restart=always
+RestartSec=30
+ExecStart=/usr/sbin/mobilebase-start 
+ExecStop=/usr/sbin/mobilebase-stop
+ExecRestart=/usr/sbin/mobilebase-restart
+
+[Install]
+WantedBy=multi-user.target

ROSCode/src/bringup/startup/readme.txt

@@ -0,0 +1,23 @@
+cd 
+sudo cp mobilebase-start /usr/sbin/
+sudo chmod +x /usr/sbin/mobilebase-start
+
+
+sudo cp mobilebase-stop /usr/sbin/
+sudo chmod +x /usr/sbin/mobilebase-stop
+
+
+sudo cp mobilebase-restart /usr/sbin/
+sudo chmod +x /usr/sbin/mobilebase-restart
+
+sudo cp  mobilebase.service /lib/systemd/system/
+
+
+----------
+sudo systemctl start mobilebase.service
+sudo systemctl stop mobilebase.service
+
+systemctl is-enabled mobilebase.service
+
+sudo systemctl enable mobilebase.service
+sudo systemctl disable mobilebase.service

ROSCode/src/camera_monitor/CMakeLists.txt

@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(camera_monitor)
+
+## 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
+  cv_bridge
+  image_transport
+  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 run_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 run_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
+#   sensor_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 run_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 you 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 camera_monitor
+#  CATKIN_DEPENDS cv_bridge image_transport opencv2 sensor_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}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/camera_monitor.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/camera_monitor.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_camera_monitor.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)

ROSCode/src/camera_monitor/launch/camera_monitor.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="camera_monitor" type="camera_monitor_node" name="camera_monitor_node" respawn="true" output="screen">
+    <param name="camera_deviceID" type="str" value="0"/>
+    <param name="camera_topic" value="/camera_monitor/image" />
+    <param name="camera_rate" value="25" />
+  </node>
+</launch>

ROSCode/src/camera_monitor/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>camera_monitor</name>
+  <version>0.0.0</version>
+  <description>The camera_monitor 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/camera_monitor</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 build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>opencv2</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>opencv2</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ROSCode/src/camera_monitor/scripts/camera.rules

@@ -0,0 +1,4 @@
+# set the udev rule , make the device_port be fixed by camera 
+#
+KERNEL=="video1", ATTRS{idVendor}=="04f2", ATTRS{idProduct}=="2803", MODE:="0777", SYMLINK+="monitor_camera"
+

ROSCode/src/camera_monitor/scripts/create_udev_rules.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+echo "remap the camera device port(video0) to  camera"
+echo "start copy camera.rules to  /etc/udev/rules.d/"
+sudo cp ./camera.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish "

ROSCode/src/camera_monitor/scripts/delete_udev_rules.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+echo "delete remap the camera device "
+echo "sudo rm /etc/udev/rules.d/camera.rules"
+sudo rm   /etc/udev/rules.d/camera.rules
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish  delete"

ROSCode/src/camera_monitor/src/camera_monitor.cpp

@@ -0,0 +1,89 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <sstream>   //for converting the command line parameter to integer
+
+using namespace std;
+
+
+static string pubCameraTopic = "/camera_monitor/image";
+static string cameraDevID = "";
+static int pubTopicRate = 25;
+
+
+int main(int argc, char** argv)
+{
+	string retryCameraID = "1";
+	ros::init(argc, argv, "image_publisher");
+	ros::NodeHandle nh;
+	image_transport::ImageTransport it(nh);
+
+	ros::param::get("~camera_deviceID", cameraDevID);
+	ros::param::get("~camera_topic", pubCameraTopic);
+	ros::param::get("~camera_rate", pubTopicRate);
+
+	image_transport::Publisher pub = it.advertise(pubCameraTopic, 2);
+
+	//Convert the passed as command line parameter index for the video device to an integer
+	std::istringstream video_sourceCmd(cameraDevID.c_str());
+	int video_source;
+
+	// Check if it is indeed a number
+	if(!(video_sourceCmd >> video_source))
+	{
+		return 1;
+	}
+	cv::VideoCapture cap(video_source);
+	cv::VideoCapture capture(video_source);
+
+	// Check if video device can be opened with the given index
+	if(!cap.isOpened())
+	{
+		ROS_ERROR("Open head camera videoID:%d error will retry...", video_source);
+
+		std::istringstream sourceCmd(retryCameraID.c_str()); 
+		if(!(sourceCmd >> video_source))
+		{
+			ROS_ERROR("make sourceCMD >> video_source error!");
+			return 1;
+		}
+		if(!capture.isOpened())
+		{
+			ROS_ERROR("Retry open head camera error!");
+			return -1;
+		}
+	}
+	ROS_INFO("Open monitor camera successfully! cameraID:%d",video_source);
+	cv::Mat frame;
+	sensor_msgs::ImagePtr msg;
+
+	ros::Rate loop_rate(pubTopicRate);
+	while (nh.ok())
+	{
+		if(video_source == 0)
+		{
+			cap >> frame;
+		}
+		else
+		{
+			capture >> frame;
+		}
+		// Check if grabbed frame is actually full with some content
+		if(!frame.empty())
+		{
+			msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
+			pub.publish(msg);
+			cv::waitKey(2);
+		}
+
+		ros::spinOnce();
+		loop_rate.sleep();
+	}
+	cap.release(); //release camera
+	capture.release();
+	cv::waitKey(2);
+
+	return 0;
+}
+

ROSCode/src/check_sensor_status/CMakeLists.txt

@@ -0,0 +1,188 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(check_sensor_status)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  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 run_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 run_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 run_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 you 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 check_sensor_status
+#  CATKIN_DEPENDS 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(check_sensor_status
+#   src/${PROJECT_NAME}/check_sensor_status.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(check_sensor_status ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(check_sensor_status_node src/main.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(check_sensor_status_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(check_sensor_status_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 check_sensor_status check_sensor_status_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_check_sensor_status.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)

ROSCode/src/check_sensor_status/include/check_sensor_status/check_sensor_status.h

@@ -0,0 +1,7 @@
+#ifndef _CHECK_SENSOR_STATUS_
+#define _CHECK_SENSOR_STATUS_
+
+#define  POWER_OFF  0
+#define  POWER_ON   1
+
+#endif

ROSCode/src/check_sensor_status/launch/check_sensor.launch

@@ -0,0 +1,43 @@
+<launch>
+  <arg name="androidLog_topic" />
+
+  <node name="check_sensor_status_node" pkg="check_sensor_status" type="check_sensor_status_node" output="screen">
+    <param name="alarmService_name" value="/mobilebase_arduino/digital_write" />
+    <param name="androidLog_topic" value="$(arg androidLog_topic)" />
+    <param name="alarm_time" type="int" value="1" />
+
+    <param name="tts_topic" type="str" value="/voice/xf_tts_topic" />
+    <param name="alarm_pin" type="int" value="51" />
+
+    <param name="lightSensor_topic" type="str" value="/head_arduino/sensor/light_sensor" />
+    <param name="isEnable_lightSensor" value="True"/>
+    <param name="warning_lightSensor" value="20"/>
+    <param name="voice_lightSensor" type="str" value="灯光太暗啦"/>
+
+    <param name="smokeSensor_topic" type="str" value="/head_arduino/sensor/smoke_sensor" />
+    <param name="isEnable_smokeSensor" value="True"/>
+    <param name="warning_smokeSensor" value="600"/>
+    <param name="voice_smokeSensor" type="str" value="煤气泄露啦"/>
+
+    <param name="noiseSensor_topic" type="str" value="/head_arduino/sensor/noise_sensor" />
+    <param name="isEnable_noiseSensor" value="True"/>
+    <param name="warning_noiseSensor" value="45"/>
+    <param name="voice_noiseSensor" type="str" value="太吵啦"/>
+
+    <param name="dropSensor_topic" type="str" value="/mobilebase_arduino/sensor/antiDropIR_front" />
+    <param name="isEnable_dropSensor" value="false" />
+    <param name="warning_dropSensor" value="1" />
+    <param name="voice_dropSensor" type="str" value="赶紧把我放下来啦"/>
+    <param name="dropSensor_interval" type="int" value="10" />
+
+    <param name="voltage_topic" type="str" value="/mobilebase_arduino/sensor/voltage_sensor" />
+    <param name="battery_pub_topic" type="str" value="/battery_percent_topic" />
+    <param name="isEnable_batCheck" value="true" />
+    <param name="max_voltage" value="16.6" />
+    <param name="min_voltage" value="12.2" />
+    <param name="warning_battery" value="20" />
+    <param name="voice_battery" type="str" value="我该充电啦，电量剩余百分之" />
+    <param name="checkBat_interval" type="int" value="60" />
+  </node>
+
+</launch>

ROSCode/src/check_sensor_status/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>check_sensor_status</name>
+  <version>0.0.0</version>
+  <description>The check_sensor_status package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="pi@todo.todo">pi</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 mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/check_sensor_status</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, 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 build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ROSCode/src/check_sensor_status/src/main.cpp

@@ -0,0 +1,220 @@
+#include <ros/ros.h>
+#include <string>
+#include <sstream>
+#include <std_msgs/String.h>
+#include "head_arduino_msgs/Analog.h"
+#include "head_arduino_msgs/DigitalWrite.h"
+#include "head_arduino_msgs/Digital.h"
+#include "ros_arduino_msgs/AnalogFloat.h"
+#include "check_sensor_status/check_sensor_status.h"
+#include "android_app_communication/androidLogMsg.h"
+
+using namespace std;
+
+class Sensor
+{
+    public:
+       string topic;
+       bool isEnable;
+       int warnValue;
+       string speakStr;
+};
+
+Sensor lightSensor;
+Sensor smokeSensor;
+Sensor noiseSensor;
+Sensor dropSensor;
+Sensor checkBatSensor;
+
+ros::Publisher tts_pub;
+ros::Publisher bat_pub;
+ros::Publisher androidLog_pub;
+ros::ServiceClient alarmClient;
+head_arduino_msgs::DigitalWrite alarmSrv;
+
+static int alarmTime = 0;
+static int alarmFlag = POWER_OFF;
+static int alarmPin = 0;
+static int dropSensorInterval = 0;
+static int checkBatInterval = 0;
+static float maxVoltage = 0.0;
+static float minVoltage = 0.0;
+
+string dropSensorLog = "检测到机器人悬空啦";
+
+void pubStrToSpeak(string input)
+{
+    std_msgs::String msg;
+    msg.data = input;
+    tts_pub.publish(msg);
+}
+
+void pubLogToAndroid(string log)
+{
+    android_app_communication::androidLogMsg msg;
+    msg.message = log;
+    androidLog_pub.publish(msg);
+}
+
+void setAlarm(int flag)
+{
+    alarmSrv.request.pin = alarmPin;
+    alarmSrv.request.value = flag;
+    if(alarmClient.call(alarmSrv))
+    {
+        ROS_INFO("Set alarm Flag [%d] successfully!", flag);
+    }
+}
+
+void lightSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
+{
+    ROS_DEBUG("Get lightSensor value:[%d]", msg->value);
+    if((msg->value < lightSensor.warnValue)&&(lightSensor.isEnable))
+    {
+        pubLogToAndroid(lightSensor.speakStr);
+        pubStrToSpeak(lightSensor.speakStr);
+    }
+}
+
+void smokeSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
+{
+    ROS_DEBUG("Get smokeSensor value:[%d]", msg->value);
+    if((msg->value > smokeSensor.warnValue)&&(smokeSensor.isEnable))
+    {
+        pubLogToAndroid(smokeSensor.speakStr);
+        noiseSensor.isEnable = false;
+        alarmFlag = POWER_ON;
+        pubStrToSpeak(smokeSensor.speakStr);
+        setAlarm(alarmFlag);
+    }
+}
+
+void noiseSensorCB(const head_arduino_msgs::Analog::ConstPtr& msg)
+{
+    ROS_DEBUG("Get noiseSensor value:[%d]", msg->value);
+    if((msg->value > noiseSensor.warnValue)&&(noiseSensor.isEnable))
+    {
+        pubLogToAndroid(noiseSensor.speakStr);
+        pubStrToSpeak(noiseSensor.speakStr);
+    }
+}
+
+void checkBatCB(const ros_arduino_msgs::AnalogFloat::ConstPtr& msg)
+{
+    int percent = 0;
+    stringstream ss;
+    string perStr;
+    static int checkTime = checkBatInterval;
+    percent = (msg->value - minVoltage)/(maxVoltage - minVoltage)*100;
+    ss<<percent;
+    ss>>perStr;
+    ROS_DEBUG("Get checkBatSensor value:[%f],percent:%d", msg->value, percent);
+    std_msgs::String batMsg;
+    batMsg.data = perStr;
+    bat_pub.publish(batMsg);
+
+    if((percent < checkBatSensor.warnValue)&&(checkBatSensor.isEnable))
+    {
+        ROS_DEBUG("Now checkBat interval:[%d]", checkTime);
+        (checkTime > 0)? (--checkTime):(checkTime = checkBatInterval);
+        if(checkTime == checkBatInterval)
+        {
+            pubLogToAndroid(checkBatSensor.speakStr);
+            noiseSensor.isEnable = false;
+            pubStrToSpeak(checkBatSensor.speakStr+perStr);
+            alarmFlag = POWER_ON;
+            setAlarm(alarmFlag);
+        }
+    }
+}
+
+void dropSensorCB(const head_arduino_msgs::Digital::ConstPtr& msg)
+{
+    ROS_DEBUG("Get drop sensor value:[%d]", msg->value);
+    static int checkTime = dropSensorInterval;
+    if((msg->value == 1)&&(dropSensor.isEnable))
+    {
+        ROS_INFO("Now dropSensor interval:[%d]", checkTime);
+        pubLogToAndroid(dropSensorLog);
+        if(checkTime == dropSensorInterval)
+        {
+            pubStrToSpeak(dropSensor.speakStr);
+        }
+        (checkTime > 0)? (--checkTime):(checkTime=dropSensorInterval);
+    }
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "check_sensor_node");
+    ros::NodeHandle nodeHandle;
+    string alarmService;
+    string ttsTopic;
+    string batPercentTopic;
+    string androidLogTopic;
+    ros::Rate loop_rate(1);
+
+    ros::param::get("~alarmService_name", alarmService);
+    ros::param::get("~androidLog_topic", androidLogTopic);
+
+    ros::param::get("~tts_topic", ttsTopic);
+    ros::param::get("~alarm_pin", alarmPin);
+    ros::param::get("~alarm_time", alarmTime);
+
+    ros::param::get("~lightSensor_topic", lightSensor.topic);
+    ros::param::get("~isEnable_lightSensor", lightSensor.isEnable);
+    ros::param::get("~warning_lightSensor", lightSensor.warnValue);
+    ros::param::get("~voice_lightSensor", lightSensor.speakStr);
+
+    ros::param::get("~smokeSensor_topic", smokeSensor.topic);
+    ros::param::get("~isEnable_smokeSensor", smokeSensor.isEnable);
+    ros::param::get("~warning_smokeSensor", smokeSensor.warnValue);
+    ros::param::get("~voice_smokeSensor", smokeSensor.speakStr);
+
+    ros::param::get("~noiseSensor_topic", noiseSensor.topic);
+    ros::param::get("~isEnable_noiseSensor", noiseSensor.isEnable);
+    ros::param::get("~warning_noiseSensor", noiseSensor.warnValue);
+    ros::param::get("~voice_noiseSensor", noiseSensor.speakStr);
+
+    ros::param::get("~dropSensor_topic", dropSensor.topic);
+    ros::param::get("~isEnable_dropSensor", dropSensor.isEnable);
+    ros::param::get("~warning_dropSensor", dropSensor.warnValue);
+    ros::param::get("~voice_dropSensor", dropSensor.speakStr);
+    ros::param::get("~dropSensor_interval", dropSensorInterval);
+
+    ros::param::get("~voltage_topic", checkBatSensor.topic);
+    ros::param::get("~isEnable_batCheck", checkBatSensor.isEnable);
+    ros::param::get("~warning_battery", checkBatSensor.warnValue);
+    ros::param::get("~voice_battery", checkBatSensor.speakStr);
+    ros::param::get("~checkBat_interval", checkBatInterval);
+    ros::param::get("~battery_pub_topic", batPercentTopic);
+    ros::param::get("~max_voltage", maxVoltage);
+    ros::param::get("~min_voltage", minVoltage);
+
+    ros::Subscriber subLightSensor = nodeHandle.subscribe(lightSensor.topic, 1, lightSensorCB);
+    ros::Subscriber subSmokeSensor = nodeHandle.subscribe(smokeSensor.topic, 1, smokeSensorCB);
+    ros::Subscriber subNoiseSensor = nodeHandle.subscribe(noiseSensor.topic, 1, noiseSensorCB);
+    ros::Subscriber subDropSensor  = nodeHandle.subscribe(dropSensor.topic,  1, dropSensorCB);
+    ros::Subscriber subBatSensor   = nodeHandle.subscribe(checkBatSensor.topic, 1, checkBatCB);
+    tts_pub = nodeHandle.advertise<std_msgs::String>(ttsTopic, 1);
+    bat_pub = nodeHandle.advertise<std_msgs::String>(batPercentTopic, 1);
+    androidLog_pub = nodeHandle.advertise<android_app_communication::androidLogMsg>(androidLogTopic, 5);
+    alarmClient = nodeHandle.serviceClient<head_arduino_msgs::DigitalWrite>(alarmService);
+
+    while(ros::ok())
+    {
+        if(alarmFlag)
+        {
+            sleep(alarmTime);
+            alarmFlag = POWER_OFF;
+            setAlarm(alarmFlag);
+            noiseSensor.isEnable = true;
+        }
+
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+
+    return 0;
+}
+

ROSCode/src/description/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(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 COMPONENTS
+  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 run_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 run_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 run_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 you 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 description
+#  CATKIN_DEPENDS 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}/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/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_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)

ROSCode/src/description/launch/description.launch

@@ -0,0 +1,15 @@
+<launch>
+    <!-- Load the URDF/Xacro model of our robot -->
+    <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find description)/urdf/mobilebase/mobilebase.xacro'" />
+
+    <param name="robot_description" command="$(arg urdf_file)" />
+    
+    <!-- Publish the robot state -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
+    <param name="publish_frequency" value="20.0"/>
+    </node>
+
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="rate" value="20.0"/>
+    </node>
+</launch>

ROSCode/src/description/launch/display.launch

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="model" default="/home/pi/corvinRobot_YOYO/ROSCode/src/description/urdf/robot.urdf"/>
+  <arg name="gui" default="False"/>
+
+  <param name="robot_description" textfile="$(arg model)"/>
+  <param name="use_gui" value="$(arg gui)"/>
+
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+  </node>
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <param name="publish_frequency" value="20.0"/>
+  </node>
+
+</launch>
+  

ROSCode/src/description/package.xml

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>description</name>
+  <version>0.0.0</version>
+  <description>The 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/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 build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ROSCode/src/description/urdf/mobilebase/base.urdf.xacro

@@ -0,0 +1,103 @@
+<?xml version="1.0"?>
+
+<robot name="base" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Define a number of dimensions using properties -->
+  <property name="base_length" value="0.165" />
+  <property name="base_radius" value="0.20" />
+  <property name="wheel_length" value="0.02" />
+  <property name="wheel_radius" value="0.060" />
+  <property name="wheel_offset_x" value="0" />
+  <property name="wheel_offset_y" value="0.17" />
+  <property name="wheel_offset_z" value="-0.038" />
+  
+  <property name="PI" value="3.1415926" />
+     
+  <!-- define a wheel -->
+  <macro name="wheel" params="suffix parent reflect color">
+    <joint name="${parent}_${suffix}_wheel_joint" type="continuous">
+      <axis xyz="0 0 1" />
+      <limit effort="100" velocity="100"/>
+      <safety_controller k_velocity="10" />
+      <origin xyz="${wheel_offset_x} ${reflect*wheel_offset_y} ${wheel_offset_z}" rpy="${reflect*PI/2} 0 0" />
+      <parent link="${parent}_link"/>
+      <child link="${parent}_${suffix}_wheel_link"/>
+    </joint>
+    <link name="${parent}_${suffix}_wheel_link">
+<!--
+ <visual>
+<origin xyz=" 0 0 0 " rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://common/urdf/meshes/wheel.STL" />
+      </geometry>
+      <material name="${color}" />
+</visual>
+-->
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
+        </geometry>
+        <material name="${color}" />
+      </visual>
+    </link>
+  </macro>
+
+  <!-- The base xacro macro -->
+  <macro name="base" params="name color">
+    <link name="${name}_link">
+
+<!--
+ <visual>
+<origin xyz=" 0 0 0 " rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://common/urdf/meshes/dash_base_all.STL" />
+      </geometry>
+      <material name="">        
+        <color
+          rgba="255 215 0 0.5"/>
+      </material>
+</visual>
+-->
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="${base_length}" radius="${base_radius}"/>
+            </geometry>
+            <material name="iRobot/LightGrey"/>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0"/>
+            <geometry>
+                <cylinder length="${base_length}" radius="${base_radius}"/>
+            </geometry>
+        </collision>
+    </link>
+  </macro>
+  
+  <link name="base_footprint">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.05 0.05 0.001" />
+        </geometry>
+        <material name="TransparentGreen" />
+      </visual>
+  </link>
+  
+  <joint name="base_joint" type="fixed">
+<!--
+    <origin xyz="0 0 0.04" rpy="0 0 0" />     
+-->  
+    <origin xyz="0 0 ${base_length/2 - wheel_offset_z}" rpy="0 0 0" />      
+    <parent link="base_footprint"/>
+    <child link="base_link" />
+  </joint>
+
+   <!-- Add the drive wheels -->
+   <wheel parent="base" suffix="l" reflect="1" color="Orange"/>
+   <wheel parent="base" suffix="r" reflect="-1" color="Orange"/>
+
+</robot>

ROSCode/src/description/urdf/mobilebase/materials.urdf.xacro

@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<robot>
+
+  <!-- Put all the color definitions in a separate file -->
+
+  <material name="Black">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  
+   <material name="TransparentBlack">
+    <color rgba="0.2 0.2 0.2 0.8"/>
+  </material>
+
+  <material name="Orange">
+    <color rgba="1.0  0.55 0.0 1.0"/>
+  </material>
+
+  <material name="Grey">
+    <color rgba="0.7 0.7 0.7 1.0"/>
+  </material>
+  
+  <material name="DarkGrey">
+    <color rgba="0.3 0.3 0.3 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="OffWhite">
+    <color rgba="0.8 0.8 0.8 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="TransparentGreen">
+    <color rgba="0.0 0.8 0.0 0.5"/>
+  </material>
+
+</robot>

ROSCode/src/description/urdf/mobilebase/mobilebase.xacro

@@ -0,0 +1,48 @@
+<?xml version="1.0"?>
+
+<robot name="box_robot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+   <!-- Define a number of dimensions using properties -->
+   <property name="torso_offset_x" value="0.0" />
+   <property name="torso_offset_y" value="0.0" />
+   <property name="torso_offset_z" value="0.23" /> <!--base_height/2+torso_height/2 -->
+
+   <!-- Include all component files -->
+   <xacro:include filename="$(find description)/urdf/mobilebase/materials.urdf.xacro" />
+   <xacro:include filename="$(find description)/urdf/mobilebase/base.urdf.xacro" />
+   <xacro:include filename="$(find description)/urdf/mobilebase/torso.urdf.xacro" />
+
+   <!-- Add the base and wheels -->
+   <base name="base" color="Black"/>
+
+   <!-- Attach the torso -->
+<!--
+   <torso name="torso" parent="base" color="Grey">
+      <origin xyz="${torso_offset_x} ${torso_offset_y} ${torso_offset_z}" rpy="0 0 0" />
+   </torso>
+-->
+
+ <joint name="laser" type="fixed">
+    <origin xyz="0.00 0.00 0.20" rpy="0 0 0" />
+    <parent link="base_link" />
+    <child link="base_laser_link" />
+  </joint>
+
+  <link name="base_laser_link">
+    <visual>
+      <geometry>
+        <box size="0.00 0.05 0.06" />
+      </geometry>
+      <material name="Green" />
+    </visual>
+    <inertial>
+      <mass value="0.000001" />
+      <origin xyz="0 0 0" />
+      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"
+        iyy="0.0001" iyz="0.0"
+        izz="0.0001" />
+    </inertial>
+  </link>
+</robot>
+
+

ROSCode/src/description/urdf/mobilebase/torso.urdf.xacro

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+
+<robot name="torso" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Define a number of dimensions using properties -->
+  <property name="torso_size_x" value="0.02" />
+  <property name="torso_size_y" value="0.02" />
+  <property name="torso_size_z" value="0.40" />
+
+  <!-- Define the torso -->
+  <macro name="torso" params="parent name color *origin">
+    <joint name="${parent}_${name}_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}_link"/>
+      <child link="${name}_link"/>
+    </joint>
+    <link name="${name}_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${torso_size_x} ${torso_size_y} ${torso_size_z}" />
+        </geometry>
+        <material name="${color}" />
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="${torso_size_x} ${torso_size_y} ${torso_size_z}" />
+        </geometry>
+      </collision>
+    </link>
+  </macro>
+
+</robot>

ROSCode/src/description/urdf/robot.urdf

@@ -0,0 +1,611 @@
+<?xml version="1.0"?>
+<robot name="my_robot">
+
+  <material name="blue">
+    <color rgba="0.1 0.1 0.75 0.9"/>
+  </material>
+  <material name="battery_blue">
+    <color rgba="0.12 0.564 1 0.9"/>
+  </material>
+  <material name="red">
+    <color rgba="0.8 0 0 1.0"/>
+  </material>
+  <material name="green">
+    <color rgba="0.04 0.64 0.27 1.0"/>
+  </material>
+  <material name="pi_green">
+    <color rgba="0 0.545 0.271 0.9"/>
+  </material>
+  <material name="black">
+    <color rgba="0.1 0.1 0.1 0.9"/>
+  </material>
+  <material name="white">
+    <color rgba="1 1 1 1.0"/>
+  </material>
+  <material name="panel_color">
+    <color rgba="0.363 0.363 0.33 0.7"/>
+  </material>
+  <material name="wheel_color">
+    <color rgba="0.2 0.2 0.2 1.0"/>
+  </material>
+  <material name="motor_color">
+    <color rgba="0.5 0.5 0.5 1.0"/>
+  </material>
+
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <cylinder length="0.005" radius="0.16"/>
+      </geometry>
+      <material name="panel_color"/>
+    </visual>
+    <inertial>
+      <mass value="3"/>
+      <inertia ixx="0.4" ixy="0.0" ixz="0.0" iyy="0.4" iyz="0.0" izz="0.2"/>
+    </inertial>
+  </link>
+
+  <link name="base_footprint">
+    <visual>
+      <geometry>
+        <box size="0.005 0.005 0.005"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin xyz="0 0 0.058"/>
+    <parent link="base_footprint"/>
+    <child link="base_link"/>
+  </joint>
+
+  <link name="drop_sensor">
+    <visual>
+      <geometry>
+        <box size="0.007 0.015 0.006"/>
+      </geometry>
+      <material name="balck"/>
+    </visual>
+  </link>
+  <joint name="base_to_dropSensor" type="fixed">
+    <origin xyz="0.155 0 -0.006"/>
+    <parent link="base_link"/>
+    <child link="drop_sensor"/>
+  </joint>
+
+  <link name="arduino_board">
+    <visual>
+      <geometry>
+        <box size="0.11 0.06 0.005"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+  </link>
+  <joint name="base_to_arduino" type="fixed">
+    <origin xyz="0 0 0.005"/>
+    <parent link="base_link"/>
+    <child link="arduino_board"/>
+  </joint>
+
+  <link name="A_wheel">
+    <visual>
+      <geometry>
+        <cylinder length="0.025" radius="0.029"/>
+      </geometry>
+      <material name="wheel_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_A_wheel" type="continuous">
+    <origin rpy="1.57079 0 0.5236" xyz="0.08725 -0.1511 -0.029"/>
+    <parent link="base_link"/>
+    <child link="A_wheel"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="A_motor">
+    <visual>
+      <geometry>
+        <cylinder length="0.12" radius="0.015"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="wheel_to_A_motor" type="fixed">
+    <origin xyz="0 0 -0.075"/>
+    <parent link="A_wheel"/>
+    <child link="A_motor"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="B_wheel">
+    <visual>
+      <geometry>
+        <cylinder length="0.025" radius="0.029"/>
+      </geometry>
+      <material name="wheel_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_B_wheel" type="continuous">
+    <origin rpy="1.57075 0 -0.5236" xyz="0.08725 0.1511 -0.029"/>
+    <parent link="base_link"/>
+    <child link="B_wheel"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="B_motor">
+    <visual>
+      <geometry>
+        <cylinder length="0.12" radius="0.015"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="wheel_to_B_motor" type="fixed">
+    <origin xyz="0 0 0.075"/>
+    <parent link="B_wheel"/>
+    <child link="B_motor"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="C_wheel">
+    <visual>
+      <geometry>
+        <cylinder length="0.025" radius="0.029"/>
+      </geometry>
+      <material name="wheel_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_C_wheel" type="continuous">
+    <origin rpy="1.57075 0 1.57" xyz="-0.1745 0 -0.029"/>
+    <parent link="base_link"/>
+    <child link="C_wheel"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <link name="C_motor">
+    <visual>
+      <geometry>
+        <cylinder length="0.12" radius="0.015"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="wheel_to_C_motor" type="fixed">
+    <origin xyz="0 0 0.075"/>
+    <parent link="C_wheel"/>
+    <child link="C_motor"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="A_pillar">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.005"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_A_pillar" type="fixed">
+      <origin xyz="0.065 -0.1126 0.025"/>
+    <parent link="base_link"/>
+    <child link="A_pillar"/>
+  </joint>
+  <link name="B_pillar">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.005"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_B_pillar" type="fixed">
+      <origin xyz="0.065 0.1126 0.025"/>
+    <parent link="base_link"/>
+    <child link="B_pillar"/>
+  </joint>
+  <link name="C_pillar">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.005"/>
+      </geometry>
+      <material name="motor_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_C_pillar" type="fixed">
+      <origin xyz="-0.135 0 0.025"/>
+    <parent link="base_link"/>
+    <child link="C_pillar"/>
+  </joint>
+
+  <link name="B_pannel" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.004" radius="0.17"/>
+      </geometry>
+      <material name="panel_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_B_pannel" type="fixed">
+    <origin xyz="0 0 0.05"/>
+    <parent link="base_link"/>
+    <child link="B_pannel"/>
+  </joint>
+
+  <link name="battery" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.15 0.135 0.04"/>
+      </geometry>
+      <material name="battery_blue"/>
+    </visual>
+  </link>
+  <joint name="battery_joint" type="fixed">
+    <origin xyz="0 0 0.022"/>
+    <parent link="B_pannel"/>
+    <child link="battery"/>
+  </joint>
+
+  <link name="A_infrared_sensor" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.03 0.015 0.015"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="Bpannel_to_A_sensor" type="fixed">
+    <origin rpy="0 0 0.5236" xyz="0.08 -0.1386 0.008"/>
+    <parent link="B_pannel"/>
+    <child link="A_infrared_sensor"/>
+  </joint>
+  <link name="B_infrared_sensor" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.015 0.03 0.015"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="Bpannel_to_B_sensor" type="fixed">
+    <origin xyz="0.16 0 0.008"/>
+    <parent link="B_pannel"/>
+    <child link="B_infrared_sensor"/>
+  </joint>
+  <link name="C_infrared_sensor" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.03 0.015 0.015"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="Bpannel_to_C_sensor" type="fixed">
+    <origin rpy="0 0 -0.5236" xyz="0.08 0.1386 0.008"/>
+    <parent link="B_pannel"/>
+    <child link="C_infrared_sensor"/>
+  </joint>
+
+  <link name="white_A_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_white_A_pillar" type="fixed">
+    <origin xyz="0.093 -0.087 0.026"/>
+    <parent link="B_pannel"/>
+    <child link="white_A_pillar"/>
+  </joint>
+  <link name="white_B_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_white_B_pillar" type="fixed">
+    <origin xyz="0.093 0.087 0.026"/>
+    <parent link="B_pannel"/>
+    <child link="white_B_pillar"/>
+  </joint>
+  <link name="white_C_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_white_C_pillar" type="fixed">
+    <origin xyz="-0.088 0.09 0.026"/>
+    <parent link="B_pannel"/>
+    <child link="white_C_pillar"/>
+  </joint>
+  <link name="white_D_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_white_D_pillar" type="fixed">
+    <origin xyz="-0.088 -0.09 0.026"/>
+    <parent link="B_pannel"/>
+    <child link="white_D_pillar"/>
+  </joint>
+
+  <link name="Pi_pannel" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.21 0.20 0.002"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="pi_pannel_to_B_pannel" type="fixed">
+    <origin xyz="0 0 0.05"/>
+    <parent link="B_pannel"/>
+    <child link="Pi_pannel"/>
+  </joint>
+
+  <link name="pi_board" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.075 0.095 0.005"/>
+      </geometry>
+      <material name="pi_green"/>
+    </visual>
+  </link>
+  <joint name="pi_board_joint" type="fixed">
+    <origin xyz="-0.06 0 0.005"/>
+    <parent link="Pi_pannel"/>
+    <child link="pi_board"/>
+  </joint>
+
+  <link name="base_imu_link" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.018 0.002"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="pi_pannel_to_IMU" type="fixed">
+    <origin xyz="0 0 0.002"/>
+    <parent link="Pi_pannel"/>
+    <child link="base_imu_link"/>
+  </joint>
+
+  <link name="white_E_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="Pi_pannel_to_white_E_pillar" type="fixed">
+    <origin xyz="0.085 0 0.026"/>
+    <parent link="Pi_pannel"/>
+    <child link="white_E_pillar"/>
+  </joint>
+  <link name="white_F_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="Pi_pannel_to_white_F_pillar" type="fixed">
+    <origin xyz="0 0.077 0.026"/>
+    <parent link="Pi_pannel"/>
+    <child link="white_F_pillar"/>
+  </joint>
+  <link name="white_G_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.05" radius="0.002"/>
+      </geometry>
+      <material name="white"/>
+    </visual>
+  </link>
+  <joint name="Pi_pannel_to_white_G_pillar" type="fixed">
+    <origin xyz="0 -0.077 0.026"/>
+    <parent link="Pi_pannel"/>
+    <child link="white_G_pillar"/>
+  </joint>
+
+  <link name="Lidar_pannel" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.0015" radius="0.085"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="lidar_pannel_to_pi_pannel" type="fixed">
+    <origin xyz="0 0 0.05"/>
+    <parent link="Pi_pannel"/>
+    <child link="Lidar_pannel"/>
+  </joint>
+
+  <link name="laser" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.042" radius="0.038"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="lidar_to_pannel" type="fixed">
+    <origin xyz="0 0 0.021"/>
+    <parent link="Lidar_pannel"/>
+    <child link="laser"/>
+  </joint>
+
+  <link name="blue_A_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.008 0.16"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_blue_A" type="fixed">
+    <origin rpy="0 0 -1.0471" xyz="0.065 -0.113 0.0815"/>
+    <parent link="B_pannel"/>
+    <child link="blue_A_pillar"/>
+  </joint>
+  <link name="blue_B_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.008 0.16"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_blue_B" type="fixed">
+    <origin rpy="0 0 1.0471" xyz="0.065 0.113 0.0815"/>
+    <parent link="B_pannel"/>
+    <child link="blue_B_pillar"/>
+  </joint>
+  <link name="blue_C_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.008 0.16"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="B_pannel_to_blue_C" type="fixed">
+    <origin xyz="-0.14 0 0.0815"/>
+    <parent link="B_pannel"/>
+    <child link="blue_C_pillar"/>
+  </joint>
+
+  <link name="left_blue_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.025 0.002"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="left_blue_pillar_joint" type="fixed">
+    <origin xyz="0 0.0165 0.025"/>
+    <parent link="blue_C_pillar"/>
+    <child link="left_blue_pillar"/>
+  </joint>
+  <link name="left_blue_A_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.04 0.01 0.003"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="left_blue_A_joint" type="fixed">
+    <origin rpy="0 0 0.611" xyz="0.017 0.013 -0.003"/>
+    <parent link="left_blue_pillar"/>
+    <child link="left_blue_A_pillar"/>
+  </joint>
+  <link name="left_blue_B_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.045 0.01 0.003"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="left_blue_B_joint" type="fixed">
+    <origin rpy="0 0 -0.851" xyz="0.032 -0.015 0.0"/>
+    <parent link="left_blue_A_pillar"/>
+    <child link="left_blue_B_pillar"/>
+  </joint>
+
+
+  <link name="right_blue_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.025 0.025 0.002"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="right_blue_pillar_joint" type="fixed">
+    <origin xyz="0 -0.0165 0.025"/>
+    <parent link="blue_C_pillar"/>
+    <child link="right_blue_pillar"/>
+  </joint>
+  <link name="right_blue_A_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.04 0.01 0.003"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="right_blue_A_joint" type="fixed">
+    <origin rpy="0 0 -0.611" xyz="0.017 -0.013 -0.003"/>
+    <parent link="right_blue_pillar"/>
+    <child link="right_blue_A_pillar"/>
+  </joint>
+  <link name="right_blue_B_pillar" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.045 0.01 0.003"/>
+      </geometry>
+      <material name="blue"/>
+    </visual>
+  </link>
+  <joint name="right_blue_B_joint" type="fixed">
+    <origin rpy="0 0 0.851" xyz="0.032 0.015 0.0"/>
+    <parent link="right_blue_A_pillar"/>
+    <child link="right_blue_B_pillar"/>
+  </joint>
+
+  <link name="C_pannel" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.004" radius="0.17"/>
+      </geometry>
+      <material name="panel_color"/>
+    </visual>
+  </link>
+  <joint name="base_to_C_pannel" type="fixed">
+    <origin xyz="0 0 0.213"/>
+    <parent link="base_link"/>
+    <child link="C_pannel"/>
+  </joint>
+
+  <link name="alarm_base" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.055" radius="0.01"/>
+      </geometry>
+      <material name="black"/>
+    </visual>
+  </link>
+  <joint name="C_pannel_to_alarmBase" type="fixed">
+    <origin xyz="-0.155 0 0.029"/>
+    <parent link="C_pannel"/>
+    <child link="alarm_base"/>
+  </joint>
+  <link name="alarm_light" type="fixed">
+    <visual>
+      <geometry>
+        <cylinder length="0.008" radius="0.014"/>
+      </geometry>
+      <material name="red"/>
+    </visual>
+  </link>
+  <joint name="alarm_base_to_alarmLight" type="fixed">
+    <origin xyz="0 0 0.03"/>
+    <parent link="alarm_base"/>
+    <child link="alarm_light"/>
+  </joint>
+
+
+</robot>

ROSCode/src/head_arduino_bridge/README.md

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

ROSCode/src/head_arduino_bridge/head_arduino_bridge/CMakeLists.txt

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

ROSCode/src/head_arduino_bridge/head_arduino_bridge/package.xml

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

ROSCode/src/head_arduino_bridge/head_arduino_firmware/CMakeLists.txt

@@ -0,0 +1,9 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(head_arduino_firmware)
+
+find_package(catkin REQUIRED)
+catkin_package(DEPENDS)
+
+install(DIRECTORY src
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

ROSCode/src/head_arduino_bridge/head_arduino_firmware/package.xml

@@ -0,0 +1,13 @@
+<package>
+  <name>head_arduino_firmware</name>
+  <version>0.2.0</version>
+  <description>
+    ROS Arduino Firmware.
+  </description>
+  <author>Patrick Goebel</author>
+  <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/head_arduino_firmware</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+</package>

ROSCode/src/head_arduino_bridge/head_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino

@@ -0,0 +1,198 @@
+/*********************************************************************
+    ROSArduinoBridge
+
+    A set of simple serial commands to control a differential drive
+    robot and receive back sensor and odometry data. Default
+    configuration assumes use of an Arduino Mega + Pololu motor
+    controller shield + Robogaia Mega Encoder shield.  Edit the
+    readEncoder() and setMotorSpeed() wrapper functions if using
+    different motor controller or encoder method.
+
+    Created for the Pi Robot Project: http://www.pirobot.org
+    and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
+
+    Authors: Patrick Goebel, James Nugen
+
+    Inspired and modeled after the ArbotiX driver by Michael Ferguson
+
+    Software License Agreement (BSD License)
+
+    Copyright (c) 2012, Patrick Goebel.
+    All rights reserved.
+ *********************************************************************/
+
+/* Serial port baud rate */
+#define BAUDRATE     57600
+
+/* Include definition of serial commands */
+#include "commands.h"
+
+/* Sensor functions */
+#include "sensors.h"
+
+/* Include servo support if required */
+#include <Servo.h>
+#include "servos.h"
+
+/* Variable initialization */
+// A pair of varibles to help parse serial commands (thanks Fergs)
+int arg = 0;
+int index = 0;
+
+// Variable to hold an input character
+char chr;
+
+// Variable to hold the current single-character command
+char cmd;
+
+// Character arrays to hold the first and second arguments
+char argv1[16];
+char argv2[16];
+
+// The arguments converted to integers
+long arg1;
+long arg2;
+
+/* Clear the current command parameters */
+void resetCommand()
+{
+  cmd = NULL;
+  memset(argv1, 0, sizeof(argv1));
+  memset(argv2, 0, sizeof(argv2));
+  arg1 = 0;
+  arg2 = 0;
+  arg = 0;
+  index = 0;
+}
+
+/* Run a command.  Commands are defined in commands.h */
+int runCommand()
+{
+  int i = 0;
+  char *p = argv1;
+  char *str;
+  int pid_args[4];
+  arg1 = atoi(argv1);
+  arg2 = atoi(argv2);
+
+  switch (cmd)
+  {
+    case GET_BAUDRATE:
+      Serial.println(BAUDRATE);
+      break;
+    case ANALOG_READ:
+      Serial.println(analogRead(arg1));
+      break;
+    case DIGITAL_READ:
+      Serial.println(digitalRead(arg1));
+      break;
+    case ANALOG_WRITE:
+      analogWrite(arg1, arg2);
+      Serial.println("OK");
+      break;
+    case DIGITAL_WRITE:
+      if (arg2 == 0) digitalWrite(arg1, LOW);
+      else if (arg2 == 1) digitalWrite(arg1, HIGH);
+      Serial.println("OK");
+      break;
+    case PIN_MODE:
+      if (arg2 == 0) pinMode(arg1, INPUT);
+      else if (arg2 == 1) pinMode(arg1, OUTPUT);
+      Serial.println("OK");
+      break;
+    case SONAR:  //'e':get head sonar value
+      Serial.println(getSonarValue(arg1));
+      break;
+    case BME_SENSOR: //'f':get temp,humi,alti
+      Serial.println(getBMESensorData(arg1));
+      break;
+    case DUST_READ:  //'g':get dust density
+      Serial.println(getDustDensity(arg1));
+      break;
+    case SERVO_WRITE:
+      servos[arg1].setTargetPosition(arg2);
+      Serial.println("OK");
+      break;
+    case SERVO_READ:
+      Serial.println(servos[arg1].getServo().read());
+      break;
+    default:
+      Serial.println("Invalid Command");
+      break;
+  }
+}
+
+/* Setup function--runs once at startup. */
+void setup()
+{
+  Serial.begin(BAUDRATE);
+  initAllSensor();
+  
+  /* Attach servos if used,first init head servo position */
+  int i;
+  for (i = 0; i < N_SERVOS; i++)
+  {
+    servos[i].initServo(
+      servoPins[i],
+      stepDelay[i],
+      servoInitPosition[i]);
+  }
+}
+
+/* Enter the main loop.  Read and parse input from the serial port
+   and run any valid commands. Run a PID calculation at the target
+   interval and check for auto-stop conditions.
+*/
+void loop() 
+{
+  while (Serial.available() > 0)
+  {
+    chr = Serial.read();  // Read the next character
+    
+    if (chr == 13)  // Terminate a command with a CR
+    {
+      if (arg == 1) argv1[index] = NULL;
+      else if (arg == 2) argv2[index] = NULL;
+      runCommand();
+      resetCommand();
+    }
+    else if (chr == ' ')  // Use spaces to delimit parts of the command
+    {
+      // Step through the arguments
+      if (arg == 0) arg = 1;
+      else if (arg == 1)  
+      {
+        argv1[index] = NULL;
+        arg = 2;
+        index = 0;
+      }
+      continue;
+    }
+    else
+    {
+      if (arg == 0) // The first arg is the single-letter command
+      {
+        cmd = chr;
+      }
+      else if (arg == 1) 
+      {
+        // Subsequent arguments can be more than one character
+        argv1[index] = chr;
+        index++;
+      }
+      else if (arg == 2) 
+      {
+        argv2[index] = chr;
+        index++;
+      }
+    }
+  }
+
+  // Sweep servos
+  int i;
+  for (i = 0; i < N_SERVOS; i++)
+  {
+    servos[i].doSweep();
+  }
+}
+

ROSCode/src/head_arduino_bridge/head_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h

@@ -0,0 +1,20 @@
+/* Define single-letter commands that will be sent by the PC over the
+   serial link.
+*/
+#ifndef COMMANDS_H
+#define COMMANDS_H
+
+#define ANALOG_READ    'a'
+#define GET_BAUDRATE   'b'
+#define PIN_MODE       'c'
+#define DIGITAL_READ   'd'
+#define SONAR          'e'
+#define BME_SENSOR     'f'
+#define DUST_READ      'g'
+#define SERVO_WRITE    's'
+#define SERVO_READ     't'
+#define DIGITAL_WRITE  'w'
+#define ANALOG_WRITE   'x'
+
+#endif
+

ROSCode/src/head_arduino_bridge/head_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h

@@ -0,0 +1,143 @@
+/* Functions for various sensor types */
+#include <DFRobot_BME280.h>
+
+#define SEA_LEVEL_PRESSURE  1013.25f
+#define BME_CS 10
+
+DFRobot_BME280 bme; //I2C
+
+#define  TEMP_FLAG  1
+#define  HUMI_FLAG  2
+#define  ALTI_FLAG  3
+
+#define  LEFT_SONAR    1
+#define  CENTER_SONAR  2
+#define  RIGHT_SONAR   3
+
+#define  A_SONAR_ECHO_PIN  12
+#define  A_SONAR_TRIG_PIN  11
+#define  B_SONAR_ECHO_PIN   8
+#define  B_SONAR_TRIG_PIN   9
+#define  C_SONAR_ECHO_PIN   2
+#define  C_SONAR_TRIG_PIN   3
+
+#define  DUST_LEDPOWER_PIN  4  //digital pin D4
+
+
+void initBME280Sensor()
+{
+  // I2c default address is 0x77, if the need to change please modify bme.begin(Addr)
+  if (!bme.begin())
+  {
+    Serial.println("No sensor device found, check line or address!");
+    while (1);
+  }
+}
+
+void initSonarPinMode()
+{
+  pinMode(A_SONAR_ECHO_PIN, INPUT);
+  pinMode(A_SONAR_TRIG_PIN, OUTPUT);
+  digitalWrite(A_SONAR_ECHO_PIN, HIGH);
+
+  pinMode(B_SONAR_ECHO_PIN, INPUT);
+  pinMode(B_SONAR_TRIG_PIN, OUTPUT);
+  digitalWrite(B_SONAR_ECHO_PIN, HIGH);
+
+  pinMode(C_SONAR_ECHO_PIN, INPUT);
+  pinMode(C_SONAR_TRIG_PIN, OUTPUT);
+  digitalWrite(C_SONAR_ECHO_PIN, HIGH);
+}
+
+void initAllSensor()
+{
+  pinMode(DUST_LEDPOWER_PIN, OUTPUT);  //init dust led power pinMode
+  initBME280Sensor();  //init temp,humi,altitude sensor
+  initSonarPinMode();  //init sonar sensor
+}
+
+int getDustDensity(int measurePin)
+{
+  int samplingTime = 280;
+  int deltaTime = 40;
+  int sleepTime = 9680;
+
+  float voMeasured = 0.0;
+  float calcVoltage = 0.0;
+  float dustDensity = 0.0; 
+
+  digitalWrite(DUST_LEDPOWER_PIN, LOW); // power on the LED
+  delayMicroseconds(samplingTime);
+
+  voMeasured = analogRead(measurePin); // read the dust value
+
+  delayMicroseconds(deltaTime);
+  digitalWrite(DUST_LEDPOWER_PIN, HIGH); // turn the LED off
+  delayMicroseconds(sleepTime);
+
+  // 0 - 5V mapped to 0 - 1023 integer values recover voltage
+  calcVoltage = voMeasured * (5.0 / 1024.0);
+
+  // linear eqaution taken from http://www.howmuchsnow.com/arduino/airquality/
+  dustDensity = 0.17 * calcVoltage - 0.1;
+  if (dustDensity < 0)
+  {
+    dustDensity = 0.0;
+  }
+  
+  return dustDensity*1000; // unit: mg/m3 -> ug/m3
+}
+
+int getBMESensorData(int flag)
+{
+  int value = 0;
+  switch (flag)
+  {
+    case TEMP_FLAG:
+      value = bme.temperatureValue();
+      break;
+    case HUMI_FLAG:
+      value = bme.humidityValue();
+      break;
+    case ALTI_FLAG:
+      value = bme.altitudeValue(SEA_LEVEL_PRESSURE);
+      break;
+    default:
+      Serial.println("Invalid Command");
+      break;
+  }
+  return value;
+}
+
+int getSonarValue(int index)
+{
+  int echoPin = 0;
+  int trigPin = 0;
+  int distance = 0;
+
+  if (index == LEFT_SONAR)
+  {
+    echoPin = A_SONAR_ECHO_PIN;
+    trigPin = A_SONAR_TRIG_PIN;
+  }
+  else if (index == CENTER_SONAR)
+  {
+    echoPin = B_SONAR_ECHO_PIN;
+    trigPin = B_SONAR_TRIG_PIN;
+  }
+  else
+  {
+    echoPin = C_SONAR_ECHO_PIN;
+    trigPin = C_SONAR_TRIG_PIN;
+  }
+  digitalWrite(trigPin, LOW); // Set the trigger pin to low for 2uS
+  delayMicroseconds(2);
+  digitalWrite(trigPin, HIGH); // Send a 10uS high to trigger ranging
+  delayMicroseconds(10);
+  digitalWrite(trigPin, LOW); // Send pin low again
+  distance = pulseIn(echoPin, HIGH, 26000); // Read in times pulse
+  distance = distance / 58;
+
+  return distance;
+}
+

ROSCode/src/head_arduino_bridge/head_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h

@@ -0,0 +1,43 @@
+#ifndef SERVOS_H
+#define SERVOS_H
+
+#define N_SERVOS 2
+
+// This delay in milliseconds determines the pause 
+// between each one degree step the servo travels.  Increasing 
+// this number will make the servo sweep more slowly.  
+// Decreasing this number will make the servo sweep more quickly.
+// Zero is the default number and will make the servos spin at
+// full speed. 150 ms makes them spin very slowly.
+int stepDelay[N_SERVOS] = {20, 20}; // ms
+
+// Pins
+byte servoPins[N_SERVOS] = {5, 6};
+
+// Initial Position
+byte servoInitPosition[N_SERVOS] = {90, 150}; //[0, 180] degrees
+
+class SweepServo
+{
+  public:
+    SweepServo();
+    void initServo(
+        int servoPin,
+        int stepDelayMs,
+        int initPosition);
+    void doSweep();
+    void setTargetPosition(int position);
+    Servo getServo();
+
+  private:
+    Servo servo;
+    int stepDelayMs;
+    int currentPositionDegrees;
+    int targetPositionDegrees;
+    long lastSweepCommand;
+};
+
+SweepServo servos[N_SERVOS];
+
+#endif
+

ROSCode/src/head_arduino_bridge/head_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino

@@ -0,0 +1,70 @@
+/***************************************************************
+   Servo Sweep - by Nathaniel Gallinger
+
+   Sweep servos one degree step at a time with a user defined
+   delay in between steps.  Supports changing direction 
+   mid-sweep.  Important for applications such as robotic arms
+   where the stock servo speed is too fast for the strength
+   of your system.
+ *************************************************************/
+
+// Constructor
+SweepServo::SweepServo()
+{
+  this->currentPositionDegrees = 0;
+  this->targetPositionDegrees  = 0;
+  this->lastSweepCommand       = 0;
+}
+
+// Init
+void SweepServo::initServo(
+         int servoPin,
+         int stepDelayMs,
+         int initPosition)
+{
+  this->servo.attach(servoPin);
+  this->stepDelayMs = stepDelayMs;
+  this->currentPositionDegrees = initPosition;
+  this->targetPositionDegrees  = initPosition;
+  this->lastSweepCommand = millis();
+  this->servo.write(this->targetPositionDegrees);
+}
+
+// Perform Sweep
+void SweepServo::doSweep()
+{
+  // Get ellapsed time
+  int delta = millis() - this->lastSweepCommand;
+
+  // Check if time for a step
+  if (delta > this->stepDelayMs) 
+  {
+    // Check step direction
+    if (this->targetPositionDegrees > this->currentPositionDegrees) 
+    {
+      this->currentPositionDegrees++;
+      this->servo.write(this->currentPositionDegrees);
+    }
+    else if (this->targetPositionDegrees < this->currentPositionDegrees) 
+    {
+      this->currentPositionDegrees--;
+      this->servo.write(this->currentPositionDegrees);
+    }
+    // if target == current position, do nothing
+    
+    this->lastSweepCommand = millis();  // reset timer
+  }
+}
+
+// Set a new target position
+void SweepServo::setTargetPosition(int position)
+{
+  this->targetPositionDegrees = position;
+}
+
+// Accessor for servo object
+Servo SweepServo::getServo()
+{
+  return this->servo;
+}
+

ROSCode/src/head_arduino_bridge/head_arduino_msgs/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(head_arduino_msgs)
+
+find_package(catkin REQUIRED COMPONENTS std_msgs message_generation)
+
+add_message_files(FILES
+                  AnalogFloat.msg
+                  Analog.msg
+                  ArduinoConstants.msg
+                  Digital.msg
+                  SensorState.msg
+                  IIC.msg
+                 )
+
+add_service_files(FILES
+                  DigitalSetDirection.srv
+                  DigitalWrite.srv
+                  DigitalRead.srv
+                  ServoRead.srv
+                  ServoWrite.srv
+                  AnalogWrite.srv
+                  AnalogRead.srv
+                 )
+
+generate_messages(   
+	DEPENDENCIES  
+	std_msgs  
+)  
+
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/Analog.msg

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/AnalogFloat.msg

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/ArduinoConstants.msg

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/Digital.msg

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/IIC.msg

@@ -0,0 +1,3 @@
+# Reading a float value 
+Header header
+uint16 value

ROSCode/src/head_arduino_bridge/head_arduino_msgs/msg/SensorState.msg

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/package.xml

@@ -0,0 +1,19 @@
+<package>
+  <name>head_arduino_msgs</name>
+  <version>0.2.0</version>
+  <description>
+    ROS Arduino Messages.
+  </description>
+  <author>Patrick Goebel</author>
+  <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
+  <license>BSD</license>
+  <url>http://ros.org/wiki/head_arduino_msgs</url>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+  
+</package>

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/AnalogRead.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/AnalogWrite.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/DigitalRead.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/DigitalSetDirection.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/DigitalWrite.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/ServoRead.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_msgs/srv/ServoWrite.srv

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

ROSCode/src/head_arduino_bridge/head_arduino_python/CMakeLists.txt

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

ROSCode/src/head_arduino_bridge/head_arduino_python/config/arduino_params.yaml

@@ -0,0 +1,51 @@
+# For a direct USB cable connection, the port name is typically
+# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
+# For a wireless connection like XBee, the port is typically
+# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.
+
+port: /dev/ttyACM0
+baud: 57600
+timeout: 0.1
+
+rate: 50
+sensorstate_rate: 10
+
+use_base_controller: False
+base_controller_rate: 10
+
+# For a robot that uses base_footprint, change base_frame to base_footprint
+base_frame: base_link
+
+# === Robot drivetrain parameters
+#wheel_diameter: 0.146
+#wheel_track: 0.2969
+#encoder_resolution: 8384 # from Pololu for 131:1 motors
+#gear_reduction: 1.0
+#motors_reversed: True
+
+# === PID parameters
+#Kp: 10
+#Kd: 12
+#Ki: 0
+#Ko: 50
+#accel_limit: 1.0
+
+# === Sensor definitions.  Examples only - edit for your robot.
+#     Sensor type can be one of the follow (case sensitive!):
+#	  * Ping
+#	  * GP2D12
+#	  * Analog
+#	  * Digital
+#	  * PololuMotorCurrent
+#	  * PhidgetsVoltage
+#	  * PhidgetsCurrent (20 Amp, DC)
+
+
+
+sensors: {
+  #motor_current_left:   {pin: 0, type: PololuMotorCurrent, rate: 5},
+  #motor_current_right:  {pin: 1, type: PololuMotorCurrent, rate: 5},
+  #ir_front_center:      {pin: 2, type: GP2D12, rate: 10},
+  #sonar_front_center:   {pin: 5, type: Ping, rate: 10},
+  arduino_led:          {pin: 13, type: Digital, rate: 5, direction: output}
+}

ROSCode/src/head_arduino_bridge/head_arduino_python/config/my_arduino_params.yaml

@@ -0,0 +1,35 @@
+# For a direct USB cable connection, the port name is typically
+# /dev/ttyACM# where is # is a number such as 0, 1, 2, etc
+# For a wireless connection like XBee, the port is typically
+# /dev/ttyUSB# where # is a number such as 0, 1, 2, etc.
+
+port: /dev/headArduino
+baud: 57600
+timeout: 0.5
+
+rate: 50
+sensorstate_rate: 10
+
+# For a robot that uses base_footprint, change base_frame to base_footprint
+base_frame: base_footprint
+
+# === Sensor definitions.  Examples only - edit for your robot.
+#     Sensor type can be one of the follow (case sensitive!):
+#	  * GP2D12
+#	  * Analog
+#	  * Digital
+#         * IIC 
+
+sensors: {
+  #arduino_led:    {pin: 13, type: Digital, rate: 1, direction: output}
+  sonar_left:      {pin: 1, type: Sonar, rate: 10},
+  sonar_center:    {pin: 2, type: Sonar, rate: 10},
+  sonar_right:     {pin: 3, type: Sonar, rate: 10},
+  light_sensor:    {pin: 0, type: Analog, rate: 0.4},
+  smoke_sensor:    {pin: 1, type: Analog, rate: 0.5},
+  noise_sensor:    {pin: 2, type: Analog, rate: 10},
+  temp_sensor:     {pin: 1, type: IIC, rate: 0.5},
+  humi_sensor:     {pin: 2, type: IIC, rate: 0.5},
+  alti_sensor:     {pin: 3, type: IIC, rate: 0.5},
+  dust_sensor:     {pin: 3, type: Dust, rate: 0.2}
+}

ROSCode/src/head_arduino_bridge/head_arduino_python/launch/head_arduino.launch

@@ -0,0 +1,5 @@
+<launch>
+   <node name="head_arduino" pkg="head_arduino_python" type="arduino_node.py" output="screen">
+      <rosparam file="$(find head_arduino_python)/config/my_arduino_params.yaml" command="load" />
+   </node>
+</launch>

ROSCode/src/head_arduino_bridge/head_arduino_python/nodes/arduino_node.py

@@ -0,0 +1,202 @@
+#!/usr/bin/env python
+
+"""
+    A ROS Node for the Arduino microcontroller
+    
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import rospy
+from head_arduino_python.arduino_driver import Arduino
+from head_arduino_python.arduino_sensors import *
+from head_arduino_msgs.srv import *
+import os, time
+import thread
+from serial.serialutil import SerialException
+
+class ArduinoROS():
+    def __init__(self):
+        rospy.init_node('head_arduino', log_level=rospy.INFO)
+
+        # Get the actual node name in case it is set in the launch file
+        self.name = rospy.get_name()
+
+        # Cleanup when termniating the node
+        rospy.on_shutdown(self.shutdown)
+
+        self.port = rospy.get_param("~port", "/dev/headArduino")
+        self.baud = int(rospy.get_param("~baud", 57600))
+        self.timeout = rospy.get_param("~timeout", 0.5)
+        self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
+
+        # Overall loop rate: should be faster than fastest sensor rate
+        self.rate = int(rospy.get_param("~rate", 50))
+        r = rospy.Rate(self.rate)
+
+        # Rate at which summary SensorState message is published. Individual sensors publish
+        # at their own rates.        
+        self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
+        
+        # Set up the time for publishing the next SensorState message
+        now = rospy.Time.now()
+        self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
+        self.t_next_sensors = now + self.t_delta_sensors
+        
+        # The SensorState publisher periodically publishes the values of all sensors on
+        # a single topic.
+        self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
+        
+        # A service to position a PWM servo
+        rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
+        
+        # A service to read the position of a PWM servo
+        rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
+        
+        # A service to turn set the direction of a digital pin (0 = input, 1 = output)
+        rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
+        
+        # A service to turn a digital sensor on or off
+        rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
+        
+        # A service to read the value of a digital sensor
+        rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 
+
+        # A service to set pwm values for the pins
+        rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
+        
+        # A service to read the value of an analog sensor
+        rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
+
+        # Initialize the controlller
+        self.controller = Arduino(self.port, self.baud, self.timeout)
+        
+        # Make the connection
+        self.controller.connect()
+        rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
+     
+        # Reserve a thread lock
+        mutex = thread.allocate_lock()
+
+        # Initialize any sensors
+        self.mySensors = list()
+        
+        sensor_params = rospy.get_param("~sensors", dict({}))
+        
+        for name, params in sensor_params.iteritems():
+            # Set the direction to input if not specified
+            try:
+                params['direction']
+            except:
+                params['direction'] = 'input'
+                
+            if params['type'] == "Sonar":
+                sensor = MySonar(self.controller, name, params['pin'], params['rate'], self.base_frame)
+            elif params['type'] == 'Digital':
+                sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+            elif params['type'] == 'Analog':
+                sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+            elif params['type'] == 'Dust':
+                sensor = DustSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+            elif params['type'] == 'IIC':
+                sensor = IICSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
+
+            try:
+                self.mySensors.append(sensor)
+                rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
+            except:
+                rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
+    
+        # Start polling the sensors and base controller
+        while not rospy.is_shutdown():
+            for sensor in self.mySensors:
+                mutex.acquire()
+                sensor.poll()
+                mutex.release()
+                    
+            # Publish all sensor values on a single topic for convenience
+            now = rospy.Time.now()
+            
+            if now > self.t_next_sensors:
+                msg = SensorState()
+                msg.header.frame_id = self.base_frame
+                msg.header.stamp = now
+                for i in range(len(self.mySensors)):
+                    msg.name.append(self.mySensors[i].name)
+                    msg.value.append(self.mySensors[i].value)
+                try:
+                    self.sensorStatePub.publish(msg)
+                except:
+                    pass
+                
+                self.t_next_sensors = now + self.t_delta_sensors
+            
+            r.sleep()
+    
+    # Service callback functions
+    def ServoWriteHandler(self, req):
+        self.controller.servo_write(req.id, req.value)
+        return ServoWriteResponse()
+    
+    def ServoReadHandler(self, req):
+        pos = self.controller.servo_read(req.id)
+        return ServoReadResponse(pos)
+    
+    def DigitalSetDirectionHandler(self, req):
+        self.controller.pin_mode(req.pin, req.direction)
+        return DigitalSetDirectionResponse()
+    
+    def DigitalWriteHandler(self, req):
+        self.controller.digital_write(req.pin, req.value)
+        return DigitalWriteResponse()
+    
+    def DigitalReadHandler(self, req):
+        value = self.controller.digital_read(req.pin)
+        return DigitalReadResponse(value)
+              
+    def AnalogWriteHandler(self, req):
+        self.controller.analog_write(req.pin, req.value)
+        return AnalogWriteResponse()
+    
+    def AnalogReadHandler(self, req):
+        value = self.controller.analog_read(req.pin)
+        return AnalogReadResponse(value)
+ 
+    def shutdown(self):
+        rospy.loginfo("Shutting down Arduino Node...")
+
+        # Stop the robot
+        try:
+            rospy.loginfo("Stopping the robot...")
+            self.cmd_vel_pub.Publish(Twist())
+            rospy.sleep(2)
+        except:
+            pass
+        
+        # Close the serial port
+        try:
+            self.controller.close()
+        except:
+            pass
+        finally:
+            rospy.loginfo("Serial port closed.")
+            os._exit(0)
+
+if __name__ == '__main__':
+    try:
+        myArduino = ArduinoROS()
+    except SerialException:
+        rospy.logerr("Serial exception trying to open port.")
+        os._exit(0)

ROSCode/src/head_arduino_bridge/head_arduino_python/package.xml

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

ROSCode/src/head_arduino_bridge/head_arduino_python/setup.py

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

ROSCode/src/head_arduino_bridge/head_arduino_python/src/head_arduino_python/arduino_driver.py

@@ -0,0 +1,351 @@
+#!/usr/bin/env python
+
+"""
+    A Python driver for the Arduino microcontroller running the
+    ROSArduinoBridge firmware.
+    
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import thread
+from math import pi as PI, degrees, radians
+import os
+import time
+import sys, traceback
+from serial.serialutil import SerialException
+from serial import Serial
+
+SERVO_MAX = 180
+SERVO_MIN = 0
+
+class Arduino:
+    ''' Configuration Parameters
+    '''    
+    N_ANALOG_PORTS = 6
+    N_DIGITAL_PORTS = 12
+    
+    def __init__(self, port="/dev/headArduino", baudrate=57600, timeout=0.5):
+        
+        self.port = port
+        self.baudrate = baudrate
+        self.timeout = timeout
+        self.writeTimeout = timeout
+        self.interCharTimeout = timeout / 30.
+    
+        # Keep things thread safe
+        self.mutex = thread.allocate_lock()
+            
+        # An array to cache analog sensor readings
+        self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
+        
+        # An array to cache digital sensor readings
+        self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
+    
+    def connect(self):
+        try:
+            print "Connecting to Arduino on port", self.port, "..."
+            self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
+            # The next line is necessary to give the firmware time to wake up.
+            time.sleep(1)
+            test = self.get_baud()
+            if test != self.baudrate:
+                time.sleep(1)
+                test = self.get_baud()   
+                if test != self.baudrate:
+                    raise SerialException
+            print "Connected at", self.baudrate
+            print "Arduino is ready."
+
+        except SerialException:
+            print "Serial Exception:"
+            print sys.exc_info()
+            print "Traceback follows:"
+            traceback.print_exc(file=sys.stdout)
+            print "Cannot connect to Arduino!"
+            os._exit(1)
+
+    def open(self): 
+        ''' Open the serial port.
+        '''
+        self.port.open()
+
+    def close(self): 
+        ''' Close the serial port.
+        '''
+        self.port.close() 
+    
+    def send(self, cmd):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        self.port.write(cmd + '\r')
+
+    def recv(self, timeout=0.5):
+        timeout = min(timeout, self.timeout)
+        ''' This command should not be used on its own: it is called by the execute commands   
+            below in a thread safe manner.  Note: we use read() instead of readline() since
+            readline() tends to return garbage characters from the Arduino
+        '''
+        c = ''
+        value = ''
+        attempts = 0
+        while c != '\r':
+            c = self.port.read(1)
+            value += c
+            attempts += 1
+            if attempts * self.interCharTimeout > timeout:
+                return None
+
+        value = value.strip('\r')
+
+        return value
+            
+    def recv_ack(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        ack = self.recv(self.timeout)
+        return ack == 'OK'
+
+    def recv_int(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        value = self.recv(self.timeout)
+        try:
+            return int(value)
+        except:
+            return None
+
+    def recv_array(self):
+        ''' This command should not be used on its own: it is called by the execute commands
+            below in a thread safe manner.
+        '''
+        try:
+            values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
+            return map(int, values)
+        except:
+            return []
+
+    def execute(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
+        '''
+        self.mutex.acquire()
+        
+        try:
+            self.port.flushInput()
+        except:
+            pass
+        
+        ntries = 1
+        attempts = 0
+        
+        try:
+            self.port.write(cmd + '\r')
+            value = self.recv(self.timeout)
+            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
+                try:
+                    self.port.flushInput()
+                    self.port.write(cmd + '\r')
+                    value = self.recv(self.timeout)
+                except:
+                    print "Exception executing command: " + cmd
+                attempts += 1
+        except:
+            self.mutex.release()
+            print "Exception executing command: " + cmd
+            value = None
+        
+        self.mutex.release()
+        return int(value)
+
+    def execute_float(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
+        '''
+        self.mutex.acquire()
+        
+        try:
+            self.port.flushInput()
+        except:
+            pass
+        
+        ntries = 1
+        attempts = 0
+        
+        try:
+            self.port.write(cmd + '\r')
+            value = self.recv(self.timeout)
+            while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
+                try:
+                    self.port.flushInput()
+                    self.port.write(cmd + '\r')
+                    value = self.recv(self.timeout)
+                except:
+                    print "Exception executing command: " + cmd
+                attempts += 1
+        except:
+            self.mutex.release()
+            print "Exception executing command: " + cmd
+            value = None
+        
+        self.mutex.release()
+        return value
+
+    def execute_array(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning an array.
+        '''
+        self.mutex.acquire()
+        
+        try:
+            self.port.flushInput()
+        except:
+            pass
+        
+        ntries = 1
+        attempts = 0
+        
+        try:
+            self.port.write(cmd + '\r')
+            values = self.recv_array()
+            while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
+                try:
+                    self.port.flushInput()
+                    self.port.write(cmd + '\r')
+                    values = self.recv_array()
+                except:
+                    print("Exception executing command: " + cmd)
+                attempts += 1
+        except:
+            self.mutex.release()
+            print "Exception executing command: " + cmd
+            raise SerialException
+            return []
+        
+        try:
+            values = map(int, values)
+        except:
+            values = []
+
+        self.mutex.release()
+        return values
+        
+    def execute_ack(self, cmd):
+        ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
+        '''
+        self.mutex.acquire()
+        
+        try:
+            self.port.flushInput()
+        except:
+            pass
+        
+        ntries = 1
+        attempts = 0
+        
+        try:
+            self.port.write(cmd + '\r')
+            ack = self.recv(self.timeout)
+            while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
+                try:
+                    self.port.flushInput()
+                    self.port.write(cmd + '\r')
+                    ack = self.recv(self.timeout)
+                except:
+                    print "Exception executing command: " + cmd
+            attempts += 1
+        except:
+            self.mutex.release()
+            print "execute_ack exception when executing", cmd
+            print sys.exc_info()
+            return 0
+        
+        self.mutex.release()
+        return ack == 'OK'   
+    
+    def get_baud(self):
+        ''' Get the current baud rate on the serial port.
+        '''
+        try:
+            return int(self.execute('b'));
+        except:
+            return None
+
+    def analog_read(self, pin):
+        return self.execute('a %d' %pin)
+    
+    def analog_write(self, pin, value):
+        return self.execute_ack('x %d %d' %(pin, value))
+    
+    def digital_read(self, pin):
+        return self.execute('d %d' %pin)
+    
+    def digital_write(self, pin, value):
+        return self.execute_ack('w %d %d' %(pin, value))
+    
+    def pin_mode(self, pin, mode):
+        return self.execute_ack('c %d %d' %(pin, mode))
+
+    def servo_write(self, id, pos):
+        ''' Usage: servo_write(id, pos)
+            Position is given in radians and converted to degrees before sending
+        '''        
+        return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
+    
+    def servo_read(self, id):
+        ''' Usage: servo_read(id)
+            The returned position is converted from degrees to radians
+        '''        
+        return radians(self.execute('t %d' %id))
+
+    def sonar(self, pin):
+        return self.execute('e %d' %pin)
+        
+    def bmesensor_read(self, pin):
+        return self.execute('f %d' %pin)
+
+    def dust_read(self, pin):
+        return self.execute('g %d' %pin)
+
+""" Basic test for connectivity """
+if __name__ == "__main__":
+    if os.name == "posix":
+        portName = "/dev/headArduino"
+    else:
+        portName = "COM43" # Windows style COM port.
+        
+    baudRate = 57600
+
+    myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
+    myArduino.connect()
+     
+    print "Sleeping for 1 second..."
+    time.sleep(1)   
+    
+    print "Reading on analog port 0", myArduino.analog_read(0)
+    print "Reading on digital port 0", myArduino.digital_read(0)
+    print "Blinking the LED 3 times"
+    for i in range(3):
+        myArduino.digital_write(13, 1)
+        time.sleep(1.0)
+    #print "Current encoder counts", myArduino.encoders()
+    
+    print "Connection test successful.",
+    
+    myArduino.stop()
+    myArduino.close()
+    
+    print "Shutting down Arduino."
+    

ROSCode/src/head_arduino_bridge/head_arduino_python/src/head_arduino_python/arduino_sensors.py

@@ -0,0 +1,284 @@
+#!/usr/bin/env python
+
+"""
+    Sensor class for the arudino_python package
+    
+    Created for the Pi Robot Project: http://www.pirobot.org
+    Copyright (c) 2012 Patrick Goebel.  All rights reserved.
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+    
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details at:
+    
+    http://www.gnu.org/licenses/gpl.html
+"""
+
+import roslib; roslib.load_manifest('head_arduino_python')
+import rospy
+from sensor_msgs.msg import Range
+from head_arduino_msgs.msg import *
+
+LOW = 0
+HIGH = 1
+
+INPUT = 0
+OUTPUT = 1
+    
+class MessageType:
+    ANALOG = 0
+    DIGITAL = 1
+    RANGE = 2
+    FLOAT = 3
+    INT = 4
+    BOOL = 5
+    
+class Sensor(object):
+    def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
+        self.controller = controller
+        self.name = name
+        self.pin = pin
+        self.rate = rate
+        self.direction = direction
+
+        self.frame_id = frame_id
+        self.value = None
+        
+        self.t_delta = rospy.Duration(1.0 / self.rate)
+        self.t_next = rospy.Time.now() + self.t_delta
+
+    def poll(self):
+        now = rospy.Time.now()
+        if now > self.t_next:
+            self.t_next = now + self.t_delta
+            if self.direction == "input":
+                try:
+                    self.value = self.read_value()
+                except:
+                    print "!!!! read_value error..."
+                    return
+            else:
+                try:
+                    self.ack = self.write_value()
+                except:
+                    return          
+
+            # For range sensors, assign the value to the range message field
+            if self.message_type == MessageType.RANGE:
+                self.msg.range = self.value
+            else:
+                self.msg.value = self.value
+
+            # Add a timestamp and publish the message
+            self.msg.header.stamp = rospy.Time.now()
+            self.pub.publish(self.msg)
+
+class AnalogSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(AnalogSensor, self).__init__(*args, **kwargs)
+                
+        self.message_type = MessageType.ANALOG
+        
+        self.msg = Analog()
+        self.msg.header.frame_id = self.frame_id
+        
+        self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
+        
+        if self.direction == "output":
+            self.controller.pin_mode(self.pin, OUTPUT)
+        else:
+            self.controller.pin_mode(self.pin, INPUT)
+
+        self.value = LOW
+        
+    def read_value(self):
+        return self.controller.analog_read(self.pin)
+    
+    def write_value(self, value):
+        return self.controller.analog_write(self.pin, value)
+    
+class AnalogFloatSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(AnalogFloatSensor, self).__init__(*args, **kwargs)
+                
+        self.message_type = MessageType.ANALOG
+        
+        self.msg = AnalogFloat()
+        self.msg.header.frame_id = self.frame_id
+        
+        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
+        
+        if self.direction == "output":
+            self.controller.pin_mode(self.pin, OUTPUT)
+        else:
+            self.controller.pin_mode(self.pin, INPUT)
+
+        self.value = LOW
+        
+    def read_value(self):
+        return self.controller.analog_read(self.pin)
+    
+    def write_value(self, value):
+        return self.controller.analog_write(self.pin, value)
+    
+class DustSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(DustSensor, self).__init__(*args, **kwargs)
+                
+        self.message_type = MessageType.ANALOG
+        
+        self.msg = Analog()
+        self.msg.header.frame_id = self.frame_id
+        
+        self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
+        self.controller.pin_mode(self.pin, INPUT)
+        self.value = LOW
+
+    def read_value(self):
+        return self.controller.dust_read(self.pin)
+        
+class DigitalSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(DigitalSensor, self).__init__(*args, **kwargs)
+        
+        self.message_type = MessageType.BOOL
+        
+        self.msg = Digital()
+        self.msg.header.frame_id = self.frame_id
+        
+        self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
+        
+        if self.direction == "output":
+            self.controller.pin_mode(self.pin, OUTPUT)
+        else:
+            self.controller.pin_mode(self.pin, INPUT)
+
+        self.value = LOW
+        
+    def read_value(self):
+        return self.controller.digital_read(self.pin)
+    
+    def write_value(self):
+        # Alternate HIGH/LOW when writing at a fixed rate
+        self.value = not self.value
+        return self.controller.digital_write(self.pin, self.value)
+    
+    
+class RangeSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(RangeSensor, self).__init__(*args, **kwargs)
+        
+        self.message_type = MessageType.RANGE
+        
+        self.msg = Range()
+        self.msg.header.frame_id = self.frame_id
+        
+        self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
+        
+    def read_value(self):
+        self.msg.header.stamp = rospy.Time.now()
+
+
+class IICSensor(Sensor):
+    def __init__(self, *args, **kwargs):
+        super(IICSensor, self).__init__(*args, **kwargs)
+
+        self.message_type = MessageType.ANALOG
+        self.msg = IIC()
+        self.msg.header.frame_id = self.frame_id
+        self.pub = rospy.Publisher("~sensor/" + self.name, IIC, queue_size=5)
+
+    def read_value(self):
+        return self.controller.bmesensor_read(self.pin)
+
+class SonarSensor(RangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(SonarSensor, self).__init__(*args, **kwargs)
+        
+        self.msg.radiation_type = Range.ULTRASOUND
+        
+class IRSensor(RangeSensor):
+    def __init__(self, *args, **kwargs):
+        super(IRSensor, self).__init__(*args, **kwargs)
+        
+        self.msg.radiation_type = Range.INFRARED
+        
+class MySonar(SonarSensor):
+    def __init__(self, *args, **kwargs):
+        super(MySonar, self).__init__(*args, **kwargs)
+        
+        self.msg.field_of_view = 0.785398163
+        self.msg.min_range = 0.25
+        self.msg.max_range = 4.0
+
+    def read_value(self):
+        cm = self.controller.sonar(self.pin)
+
+        #Convert it to meters for ROS
+        distance = cm/100.0
+        return distance
+
+class GP2D12(IRSensor):
+    def __init__(self, *args, **kwargs):
+        super(GP2D12, self).__init__(*args, **kwargs)
+        
+        self.msg.field_of_view = 0.001
+        self.msg.min_range = 0.10
+        self.msg.max_range = 0.80
+        
+    def read_value(self):
+        value = self.controller.analog_read(self.pin)
+        
+        if value <= 3.0:
+            return self.msg.max_range
+        
+        try:
+            distance = (6787.0 / (float(value) - 3.0)) - 4.0
+        except:
+            return self.msg.max_range
+            
+        # Convert to meters
+        distance /= 100.0
+        
+        # If we get a spurious reading, set it to the max_range
+        if distance > self.msg.max_range: distance = self.msg.max_range
+        if distance < self.msg.min_range: distance = self.msg.max_range
+        
+        return distance
+    
+class PololuMotorCurrent(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(PololuMotorCurrent, self).__init__(*args, **kwargs)
+        
+    def read_value(self):
+        # From the Pololu source code
+        milliamps = self.controller.analog_read(self.pin) * 34
+        return milliamps / 1000.0
+    
+class PhidgetsVoltage(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(PhidgetsVoltage, self).__init__(*args, **kwargs)
+        
+    def read_value(self):
+        # From the Phidgets documentation
+        voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.)
+        return voltage
+    
+class PhidgetsCurrent(AnalogFloatSensor):
+    def __init__(self, *args, **kwargs):
+        super(PhidgetsCurrent, self).__init__(*args, **kwargs)
+        
+    def read_value(self):
+        # From the Phidgets documentation for the 20 amp DC sensor
+        current = 0.05 * (self.controller.analog_read(self.pin) - 500.)
+        return current
+    
+if __name__ == '__main__':
+    myController = Controller()
+    mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10)
+            

ROSCode/src/head_arduino_bridge/scripts/create_udev_rules.sh

@@ -0,0 +1,13 @@
+#!/bin/bash
+
+echo "remap the device serial port(ttyUSBX) to  arduino"
+echo "arduino usb connection as /dev/arduino , check it using the command : ls -l /dev|grep ttyUSB"
+echo "start copy head_arduino.rules to  /etc/udev/rules.d/"
+sudo cp ./head_arduino.rules  /etc/udev/rules.d
+echo " "
+echo "Restarting udev"
+echo ""
+sudo udevadm control --reload-rules
+sudo service udev restart
+sudo udevadm trigger
+echo "finish "

ROSCode/src/head_arduino_bridge/scripts/delete_udev_rules.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+echo "delete remap the device serial port(ttyUSBX) to  arduino"
+echo "sudo rm /etc/udev/rules.d/head_arduino.rules"
+sudo rm /etc/udev/rules.d/head_arduino.rules
+echo " "
+echo "Restarting udev"
+echo ""
+sudo service udev reload
+sudo service udev restart
+echo "finish  delete"

ROSCode/src/head_arduino_bridge/scripts/head_arduino.rules

@@ -0,0 +1,4 @@
+# set the udev rule , make the device_port be fixed by arduino
+#
+KERNELS=="1-1.5.4", ATTRS{idVendor}=="1a86", ATTRS{idProduct}=="7523", MODE:="0777", SYMLINK+="headArduino"
+

ROSCode/src/navigation/CMakeLists.txt

@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(navigation)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  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 run_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 run_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 run_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 you 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 navigation
+#  CATKIN_DEPENDS 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}/navigation.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/navigation_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_navigation.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)

ROSCode/src/navigation/config/mobilebase/base_local_planner_params.yaml

@@ -0,0 +1,40 @@
+controller_frequency: 10.0
+recovery_behavior_enabled: false
+clearing_rotation_allowed: false
+
+TrajectoryPlannerROS:
+   max_vel_x: 0.3
+   min_vel_x: 0.15
+   max_vel_y: 0.2  # zero for a differential drive robot
+   min_vel_y: 0.1
+   max_vel_theta: 1.5
+   min_in_place_vel_theta: 0.8
+   escape_vel: -0.15
+   acc_lim_x: 0.05
+   acc_lim_y: 0.05 # zero for a differential drive robot
+   acc_lim_theta: 1.0
+
+   holonomic_robot: true
+   yaw_goal_tolerance: 0.5 # about 30 degrees
+   xy_goal_tolerance: 0.15  # 15 cm
+   latch_xy_goal_tolerance: false
+   pdist_scale: 0.8
+   gdist_scale: 0.6
+   meter_scoring: true
+
+   heading_lookahead: 0.325
+   heading_scoring: false
+   heading_scoring_timestep: 0.8
+   occdist_scale: 0.1
+   oscillation_reset_dist: 0.05
+   publish_cost_grid_pc: false
+   prune_plan: true
+
+   sim_time: 1.0
+   sim_granularity: 0.025
+   angular_sim_granularity: 0.025
+   vx_samples: 8
+   vy_samples: 8 # zero for a differential drive robot
+   vtheta_samples: 20
+   dwa: true
+   simple_attractor: false

ROSCode/src/navigation/config/mobilebase/costmap_common_params.yaml

@@ -0,0 +1,8 @@
+obstacle_range: 2.0
+raytrace_range: 3.0
+robot_radius: 0.18
+inflation_radius: 0.1
+max_obstacle_height: 0.4
+min_obstacle_height: 0.05
+observation_sources: scan
+scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0.3}

ROSCode/src/navigation/config/mobilebase/global_costmap_params.yaml

@@ -0,0 +1,10 @@
+global_costmap:
+   global_frame: /map
+   robot_base_frame: /base_footprint
+   update_frequency: 2.0
+   publish_frequency: 0
+   static_map: true
+   rolling_window: false
+   resolution: 0.02
+   transform_tolerance: 3.0
+   map_type: costmap

ROSCode/src/navigation/config/mobilebase/local_costmap_params.yaml

@@ -0,0 +1,12 @@
+local_costmap:
+   global_frame: /map
+   robot_base_frame: /base_footprint
+   update_frequency: 3.0
+   publish_frequency: 1.0
+   static_map: false
+   rolling_window: true
+   width: 1.0
+   height: 1.0
+   resolution: 0.05   #should same as map.yaml file
+   transform_tolerance: 3.0
+   map_type: costmap

ROSCode/src/navigation/config/nav_obstacles_params.yaml

@@ -0,0 +1,5 @@
+TrajectoryPlannerROS:
+  max_vel_x: 0.3
+  pdist_scale: 0.8
+  gdist_scale: 0.4
+  

ROSCode/src/navigation/config/nav_test_params.yaml

@@ -0,0 +1,6 @@
+TrajectoryPlannerROS:
+  yaw_goal_tolerance: 6.28 # We don't care about orientation
+  xy_goal_tolerance: 0.1   # 10 cm
+  pdist_scale: 0.8
+  gdist_scale: 0.4
+  occdist_scale: 0.1

ROSCode/src/navigation/launch/amcl_omni.launch

@@ -0,0 +1,42 @@
+<launch>
+<node pkg="amcl" type="amcl" name="amcl">
+  <!-- Publish scans from best pose at a max of 10 Hz -->
+  <param name="base_frame_id" value="base_footprint"/>
+  <param name="global_frame_id" value="map"/>
+  <param name="use_map_topic" value="true"/>
+  <param name="odom_model_type" value="omni"/>
+  <param name="odom_alpha5" value="0.1"/>
+  <param name="transform_tolerance" value="0.5" />
+  <param name="gui_publish_rate" value="10.0"/>
+  <param name="laser_max_beams" value="300"/>
+  <param name="min_particles" value="500"/>
+  <param name="max_particles" value="5000"/>
+  <param name="kld_err" value="0.1"/>
+  <param name="kld_z" value="0.99"/>
+  <param name="odom_alpha1" value="0.1"/>
+  <param name="odom_alpha2" value="0.1"/>
+
+  <!-- translation std dev, m -->
+  <param name="odom_alpha3" value="0.1"/>
+  <param name="odom_alpha4" value="0.1"/>
+  <param name="laser_z_hit" value="0.9"/>
+  <param name="laser_z_short" value="0.05"/>
+  <param name="laser_z_max" value="0.05"/>
+  <param name="laser_z_rand" value="0.5"/>
+  <param name="laser_sigma_hit" value="0.2"/>
+  <param name="laser_lambda_short" value="0.1"/>
+  <param name="laser_lambda_short" value="0.1"/>
+  <param name="laser_model_type" value="likelihood_field"/>
+  <!-- <param name="laser_model_type" value="beam"/> -->
+  <param name="laser_min_range" value="0.14"/>
+  <param name="laser_max_range" value="7.5"/>
+  <param name="laser_likelihood_max_dist" value="2.0"/>
+  <param name="update_min_d" value="0.2"/>
+  <param name="update_min_a" value="0.5"/>
+  <param name="odom_frame_id" value="odom"/>
+  <param name="resample_interval" value="1"/>
+  <param name="transform_tolerance" value="3.0"/>
+  <param name="recovery_alpha_slow" value="0.0"/>
+  <param name="recovery_alpha_fast" value="0.0"/>
+</node>
+</launch>

ROSCode/src/navigation/launch/amcl_start.launch

@@ -0,0 +1,8 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find navigation)/maps/mymap.yaml" />
+
+  <!-- Run amcl -->
+  <include file="$(find navigation)/launch/amcl_omni.launch" />
+
+</launch>

ROSCode/src/navigation/launch/gmapping_start.launch

@@ -0,0 +1,35 @@
+<launch>  
+<param name="use_sim_time" value="false"/>  
+<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+  <!--remap from="scan" to="base_scan"/-->  
+  <param name="map_update_interval" value="5.0"/>  
+  <param name="maxUrange" value="7.5"/>  
+  <param name="sigma" value="0.05"/>  
+  <param name="kernelSize" value="1"/>  
+  <param name="lstep" value="0.05"/>  
+  <param name="astep" value="0.05"/>  
+  <param name="iterations" value="5"/>  
+  <param name="lsigma" value="0.075"/>  
+  <param name="ogain" value="3.0"/>  
+  <param name="lskip" value="0"/>  
+  <param name="minimumScore" value="50"/>  
+  <param name="srr" value="0.1"/>  
+  <param name="srt" value="0.2"/>  
+  <param name="str" value="0.1"/>  
+  <param name="stt" value="0.2"/>  
+  <param name="linearUpdate" value="1.0"/>  
+  <param name="angularUpdate" value="0.5"/>  
+  <param name="temporalUpdate" value="3.0"/>  
+  <param name="resampleThreshold" value="0.5"/>  
+  <param name="particles" value="30"/>  
+  <param name="xmin" value="-5.0"/>  
+  <param name="ymin" value="-5.0"/>  
+  <param name="xmax" value="5.0"/>  
+  <param name="ymax" value="5.0"/>  
+  <param name="delta" value="0.05"/>  
+  <param name="llsamplerange" value="0.01"/>  
+  <param name="llsamplestep" value="0.01"/>  
+  <param name="lasamplerange" value="0.005"/>  
+  <param name="lasamplestep" value="0.005"/>  
+</node>  
+</launch>  

ROSCode/src/navigation/launch/move_base.launch

@@ -0,0 +1,9 @@
+<launch>
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <rosparam file="$(find navigation)/config/mobilebase/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find navigation)/config/mobilebase/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find navigation)/config/mobilebase/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find navigation)/config/mobilebase/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find navigation)/config/mobilebase/base_local_planner_params.yaml" command="load" />
+  </node>
+</launch>

ROSCode/src/navigation/launch/move_base_blank_map.launch

@@ -0,0 +1,14 @@
+<launch>
+  <!-- Launch move_base and load all navigation parameters -->
+  <include file="$(find navigation)/launch/move_base.launch" />
+  
+  <!-- Run the map server with a blank map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find navigation)/maps/blank_map.yaml" />
+
+  <!-- run amcl -->
+  <include file="$(find amcl)/examples/amcl_omni.launch" />
+
+  <!-- Run a static transform between /odom and /map -->
+  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
+
+</launch>

ROSCode/src/navigation/launch/move_base_obstacle.launch

@@ -0,0 +1,14 @@
+<launch>
+
+  <include file="$(find rbx1_nav)/launch/move_base.launch" />
+  
+  <!-- Run the map server with a blank map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map_with_obstacle.yaml"/>
+    
+  <!-- Run a static transform between /odom and /map -->
+  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
+  
+  <!-- For the TurtleBot, we need Run a static transform between /odom and /odom_combined -->
+  <node pkg="tf" type="static_transform_publisher" name="odom_combined_broadcaster" args="0 0 0 0 0 0 /odom /odom_combined 100" />
+    
+</launch>

ROSCode/src/navigation/launch/view_blind_nav.launch

@@ -0,0 +1,6 @@
+<!--
+  Used for visualising the turtlebot navigating locally only with bumpers and cliff sensors. 
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_rviz_launchers)/rviz/blind_nav.rviz"/>
+</launch>

ROSCode/src/navigation/launch/view_model.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
+    <param name="use_gui" value="true"/>
+  </node>
+  
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation)/rviz/model.rviz" />
+</launch>

ROSCode/src/navigation/launch/view_navigation.launch

@@ -0,0 +1,6 @@
+<!--
+  Used for visualising the robot while building a map or navigating with the ros navistack.
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation)/rviz/navigation.rviz"/>
+</launch>

ROSCode/src/navigation/launch/view_navigation_app.launch

@@ -0,0 +1,9 @@
+<!--
+  Used for visualising the turtlebot while building a map or navigating with the ros navistack.
+  
+  Use this version of view_navigation.launch whenever the navigation related topics lay inside
+  an application namespace (that is, navigation was launched by the app manager).
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find turtlebot_rviz_launchers)/rviz/navigation_app.rviz"/>
+</launch>

ROSCode/src/navigation/launch/view_robot.launch

@@ -0,0 +1,8 @@
+<!--
+  Used for visualising robot in action.  
+  
+  It requires minimal.launch and optionally 3dsensor.launch to already be up and running.
+ -->
+<launch>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation)/rviz/robot.rviz" />
+</launch>

ROSCode/src/navigation/launch/view_teleop_navigation.launch

@@ -0,0 +1,7 @@
+<launch>
+  <!-- turtlebot_teleop_key already has its own built in velocity smoother -->
+  <node pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" name="teleop_keyboard"  output="screen">
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find navigation)/rviz/navigation.rviz"/>
+</launch>

ROSCode/src/navigation/maps/blank_map.yaml

@@ -0,0 +1,6 @@
+image: blank_map.pgm
+resolution: 0.01
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.196 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

ROSCode/src/navigation/maps/blank_map_with_obstacle.yaml

@@ -0,0 +1,6 @@
+image: blank_map_with_obstacle.pgm
+resolution: 0.01
+origin: [-2.5, -2.5, 0]
+occupied_thresh: 0.65
+free_thresh: 0.195 # Taken from the Willow Garage map in the turtlebot_navigation package
+negate: 0

ROSCode/src/navigation/maps/mymap.yaml

@@ -0,0 +1,7 @@
+image: lastMap.pgm
+resolution: 0.050000
+origin: [-27.400000, -17.800000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+