Initial commit

.gitignore

+*.pyc

minesearch_project/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(minesearch_project)
+
+## 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
+  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
+#  Circle.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
+#   rospy
+#   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 your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES cs4313_project
+  CATKIN_DEPENDS message_runtime
+#  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}/cs4313_project.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/cs4313_project_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
+  src/minesearch_project/project_robot.py
+  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_cs4313_project.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)

minesearch_project/launch/loadMine.xml

+<launch>
+  <!-- send table.xml to param server -->
+  <param name="mine1" command="$(find xacro)/xacro --inorder $(find minesearch_project)/models/mine.xacro" />
+  <param name="mine2" command="$(find xacro)/xacro --inorder $(find minesearch_project)/models/mine.xacro" />
+  <param name="mine3" command="$(find xacro)/xacro --inorder $(find minesearch_project)/models/mine.xacro" />
+  <param name="mine4" command="$(find xacro)/xacro --inorder $(find minesearch_project)/models/mine.xacro" />
+  <param name="mine5" command="$(find xacro)/xacro --inorder $(find minesearch_project)/models/mine.xacro" />
+</launch>
+

minesearch_project/launch/minefield.launch

+<?xml version="1.0"?>
+<launch>
+  <arg name="x1" default="36"/>
+  <arg name="y1" default="39"/>
+  <arg name="x2" default="66"/>
+  <arg name="y2" default="26"/>
+  <arg name="loadMine3" default="1"/>
+  <arg name="x3" default="65"/>
+  <arg name="y3" default="5"/>
+  <arg name="loadMine4" default="1"/>
+  <arg name="x4" default="31"/>
+  <arg name="y4" default="10"/>
+  <arg name="loadMine5" default="1"/>
+  <arg name="x5" default="2"/>
+  <arg name="y5" default="40"/>
+
+  <include file="$(find minesearch_project)/launch/loadMine.xml" />
+  <include file="$(find pioneer_launch)/launch/upload_pioneer3at.xml" /> 
+  <include file="$(find cs4313_worlds)/launch/hokuyoGmapper.launch" />
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="debug" value="false"/>
+    <arg name="world_name" value="$(find minesearch_project)/worlds/minefield.world"/>
+  </include>
+
+  <!-- launch the circle-finder node to detect and publish circular objects -->
+  <node name="circleFinder" pkg="minesearch_project" type="circleFinder.py" />  
+
+  <!-- load mine-like objects into the scene based on argument values -->
+  <node name="spawn_mine1" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x1) -y $(arg y1) -urdf -param mine1 -model mine1" respawn="false" output="screen" />
+  <node name="spawn_mine2" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x2) -y $(arg y2) -urdf -param mine2 -model mine2" respawn="false" output="screen" />
+  <group if="$(arg loadMine3)">
+    <node name="spawn_mine3" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x3) -y $(arg y3) -urdf -param mine2 -model mine3" respawn="false" output="screen" />
+  </group>
+  <group if="$(arg loadMine4)">
+    <node name="spawn_mine4" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x4) -y $(arg y4) -urdf -param mine2 -model mine4" respawn="false" output="screen" />
+  </group>  
+  <group if="$(arg loadMine5)">
+    <node name="spawn_mine5" pkg="gazebo_ros" type="spawn_model" args="-x $(arg x5) -y $(arg y5) -urdf -param mine2 -model mine5" respawn="false" output="screen" />
+  </group>  
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="spawn_pioneer" pkg="gazebo_ros" type="spawn_model" args="-z 0.051 -urdf -param robot_description -model robot_description" respawn="false" output="screen" />
+
+</launch>

minesearch_project/models/mine.xacro

+<?xml version="1.0"?>
+<robot name="mine" xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <gazebo reference="mineLink">
+    <material>Gazebo/GreenGlow</material>
+  </gazebo>
+
+  <!-- Used for fixing robot to Gazebo 'base_link' -->
+  <link name="world"/>
+
+  <joint name="fixed" type="floating">
+    <parent link="world"/>
+    <child link="mineLink"/>
+  </joint>
+
+  <link name="mineLink">
+    <collision>
+      <origin xyz="0 0 0.3" rpy="0 0 0"/>
+      <geometry>
+	<cylinder radius="0.25" length="0.6"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0 0.3" rpy="0 0 0"/>
+      <geometry>
+	<cylinder radius="0.25" length="0.6"/>
+      </geometry>
+    </visual>
+
+    <inertial>
+      <origin xyz="0 0 0.5" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+	  ixx="1.0" ixy="0.0" ixz="0.0"
+	  iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+</robot>

minesearch_project/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>minesearch_project</name>
+  <version>0.0.0</version>
+  <description>The cs4313 minesearch_project package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="cs4313@todo.todo">cs4313</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/cs4313_project</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>rospy</depend>
+  <depend>roscpp</depend>
+  <depend>rosmsg</depend>
+  <depend>message_runtime</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

minesearch_project/setup.py

+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+setup_args = generate_distutils_setup(
+    packages=['minesearch_project'],
+    package_dir={'' : 'src'}
+)
+
+setup(**setup_args)
+

minesearch_project/src/circleFinder.py

+#!/usr/bin/env python
+
+import math
+
+import rospy
+import sensor_msgs.msg as sensor_msgs
+
+import cs4313_utilities.shared_robot_classes as src
+import cs4313_utilities.robot_math as rm
+import project_msgs.msg as project_msgs
+
+
+# Class for a ROS node that identifies circles in the laser scan
+class CircleFinder:
+
+    # Initializer registers as a ROS node, subscribes to the laser scan topic
+    # and identifies itself as a publisher to the circles topic
+    # @param nodeName: name of the circlefinder node
+    # @param scanTopic: topic to which laser scan data is published
+    # @param circleTopic: topic to which circle messages will be published
+    def __init__(self, nodeName="circleFinder", scanTopic="scan", \
+                 circleTopic="circles"):
+        rospy.init_node(nodeName)
+        self.scan = src.LaserData()
+        self.message = project_msgs.Circle()
+        self.message.header.seq = 0
+        self.message.header.frame_id = "laser"
+        self.publisher = rospy.Publisher(circleTopic, project_msgs.Circle)
+        rospy.Subscriber(scanTopic, sensor_msgs.LaserScan, self.processLaser)
+
+
+    # Processes new laser data when it arrives.  Tests each interval of
+    # continuity to see if it equates to a circle.  The circle passing through
+    # each interval's endpoints and center point is calculated, and then a
+    # test point on each side of the arc is tested to see if it is on the same
+    # circle.  If so, the circle is considered "confirmed" and it is published.
+    # Intervals of continuity of fewer than five points are not considered.
+    # @param scanData: laser scan data
+    def processLaser(self, scanData):
+        self.scan.update(scanData)
+        for interval in self.scan.intervals:
+            print "Interval"
+            if (interval[1] - interval[0]) >= 5:
+                leftAngle = self.scan.minAngle + interval[0] * self.scan.increment
+                leftPt = (self.scan.ranges[interval[0]] * math.cos(leftAngle), \
+                          self.scan.ranges[interval[0]] * math.sin(leftAngle))
+
+                middle = (interval[0] + interval[1]) / 2
+                middleAngle = self.scan.minAngle + middle * self.scan.increment
+                middlePt = (self.scan.ranges[middle] * math.cos(middleAngle), \
+                            self.scan.ranges[middle] * math.sin(middleAngle))
+
+                rightAngle = self.scan.minAngle + interval[1] * self.scan.increment
+                rightPt = (self.scan.ranges[interval[1]] * math.cos(rightAngle), \
+                           self.scan.ranges[interval[1]] * math.sin(rightAngle))
+
+                leftTest = (interval[0] + middle) / 2
+                leftTestAngle = self.scan.minAngle + leftTest * self.scan.increment
+                leftTestPt = (self.scan.ranges[leftTest] * math.cos(leftTestAngle), \
+                              self.scan.ranges[leftTest] * math.sin(leftTestAngle))
+
+                rightTest = (middle + interval[1]) / 2
+                rightTestAngle = self.scan.minAngle + rightTest * self.scan.increment
+                rightTestPt = (self.scan.ranges[rightTest] * math.cos(rightTestAngle), \
+                               self.scan.ranges[rightTest] * math.sin(rightTestAngle))
+
+                circle = rm.compute_circle(leftPt, middlePt, rightPt)
+                if (circle and (circle[1] < 50.0) and \
+                    (abs(rm.cartesian_distance(circle[0], leftTestPt) - \
+                                               circle[1]) < 0.05) and \
+                    (abs(rm.cartesian_distance(circle[0], rightTestPt) - \
+                                               circle[1]) < 0.05)):
+                    self.message.header.seq += 1
+                    self.message.header.stamp = rospy.Time.now()
+                    self.message.x = circle[0][0]
+                    self.message.y = circle[0][1]
+                    self.message.radius = circle[1]
+                    self.publisher.publish(self.message)
+
+
+# Main Program (primarily for testing purposes for now)
+if __name__ == "__main__":
+    finder = CircleFinder()
+    rospy.spin()
+
+

minesearch_project/src/minesearch_project/project_robot.py

+#!/usr/bin/env python
+
+# Standard python imports
+import math
+
+# ROS imports
+import rospy
+import tf
+import geometry_msgs.msg as geometry_msgs
+import nav_msgs.msg as nav_msgs
+import sensor_msgs.msg as sensor_msgs
+import geometry_msgs.msg as geometry_msgs
+
+# CS4313 library imports
+import cs4313_utilities.robot as robot
+import cs4313_utilities.robot_math as rm
+import cs4313_utilities.shared_robot_classes as src
+import cs4313_utilities.quaternion_math as quat
+
+# Constants for use throughout the class
+
+WALL_FOLLOW_TRANSITION_STEPS = 20
+
+
+class SensingRobot(robot.SimpleRobot):
+    ''' Class extending the SimpleRobot class to provide additional capability.
+    and control modes (potential field and bug2) based on laser scan data.
+
+    Member variables:
+      navMode: enumeration for which navigation mode to use
+      mLine: directed line used for line following
+      sideOfMLine: enumeration for what side of the m-line the vehicle is on
+      wallFollowCtr: delay counter before testing m-line distance
+      turnAccumulator: accumulated turn during wall following
+      thetaLast: used to integrate turn accumulator
+
+    Member variables (inherited from SimpleRobot):
+      state: current state containing pose, velocity, and odometry values
+      laserData: current laser data
+      controlMode: enumeration for which control mode to use
+      navMode: enumeration for which navigation mode to use
+      controllers: dictionary of control functions
+      waypoint: Cartesian coordinates of the current waypoint
+      waypointDistance: distance from the current waypoint
+      mLine: directed line used for line following
+      cmd_vel: ROS message object for velocity commands
+      cmdPub: ROS publisher object for velocity commands
+      isAtWaypoint: Boolean indicator for reaching a waypoint
+      sideOfMLine: enumeration for what side of the m-line the vehicle is on
+      wallFollowCtr: delay counter before testing m-line distance
+      turnAccumulator: accumulated turn during wall following
+      thetaLast: used to integrate turn accumulator
+      baseToOdomTF: ROS broadcaster to publish odometry transforms
+      mapToOdomTF: ROS listener for map transforms
+      timer: ROS timer for control loop iteration
+
+    Class methods (inherited from SimpleRobot):
+      setControlConstants: override of parent class method for additional constants
+      pfWaypointControl: uses a potential field to compute a command to a waypoint
+      bug2WaypointControl: uses the Bug 2 algorithm to compute a command to a waypoint
+      wallFollowingControl: computes a command to follow (parallel) a detected wall
+      lineTrackingControl: computes a command to follow a line
+
+    Class methods (inherited from SimpleRobot):
+      spinOnce: runs a single iteration of the vehicle's control loop
+      setWaypoint: sets the current waypoint to a new location
+      waypointCaptured: Boolean test for reaching the waypoint
+      processLaser: ROS callback for updating laser scan data
+      processGroundTruth: ROS callback for updating state based on ground truth data
+      processOdometry: ROS callback for updating state based on odometry data
+      clampedMaxXDotCommand: computes a max fwd speed command to account for heading error
+      setVelocityCommands: sets the velocity command based on heading and distance error
+      pdThetaCommand: computes a proportional-derivative turn rate command
+      pdXDotCommand: computes a proportional-derivative forward speed command
+      pdWaypointControl: computes a proportional-derivative command directly to a waypoint
+    '''
+
+    def __init__(self, odomTopic="odom", laserTopic="scan", \
+                       cmdTopic="cmd_vel", tfTopic="tf"):
+        ''' Initializes the object member variables, registers the object
+        subscriptions and publishers with ROS
+        @param odomTopic: topic name to subscribe to for odometry data
+        @param laserTopic: topic name to subscribe to tfor laser scan data
+        @param cmdTopic: topic name to publish velocity commands to
+        @param tfTopic: topic to which the robot will publish its odom to map tf
+        '''
+        robot.SimpleRobot.__init__(self, odomTopic, laserTopic, cmdTopic, tfTopic)
+
+        # Vehicle controllers
+        self.controlMode = src.BUG2_CONTROL # current control mode
+        self.navMode = src.LINE_FOLLOWING # Current navigation mode
+        self.controllers[src.PF_CONTROL] = self.pfWaypointControl
+        self.controllers[src.BUG2_CONTROL] = self.bug2WaypointControl
+
+        # Command info (current commands)
+        self.mLine = src.DirectedLine((0.0, 0.0), (0.0, 0.0))
+        self.sideOfMLine = 0        # Which side of the m-line for Bug2
+        self.wallFollowCtr = 0      # Delay before testing m-line distance
+        self.turnAccumulator = 0.0  # Accumulated turn during wall following
+        self.thetaLast = 0.0        # Used to integrate turn accumulator
+
+
+
+    def setControlConstants(self, newCaptureDist=1.5, newWallFollowDist=1.5, \
+                            newInterceptDist=2.0, newMaxXDot=1.0, newMaxThetaDot=0.77, \
+                            newXCmdC1=2.0, newXCmdC2=1.0, newPDThetaC1=3.0, \
+                            newPDThetaC2=1.0, newPFZeta=7.0, newPFEta=0.2):
+        '''Overrides parent class method to include additional control constants
+        for potential field and bug algorithm implementations
+        @param newCaptureDist: updated distance to consider a waypoint "achieved"
+        @param newWallFollowDist: updated wall following distance
+        @param newInterceptDist: distance along the line to intercept when tracking
+        @param newMaxXDot: updated max forward commanded velocity
+        @param newMaxThetaDot: updated max commanded angular (radian) velocity
+        @param newXDotCmdC: updated coefficient for forward speed command
+        @param newPDThetaC1: updated turn speed PD control error multiple
+        @param newPDThetaC2: updated turn speed PD control rate (damping) multiple
+        @param newPFZeta:  updated potential field zeta (attractive) coefficient
+        @param newPFEta: updated potential field eta (repulsive) coefficient
+        '''
+        robot.SimpleRobot.setControlConstants(self, newCaptureDist, newMaxXDot, \
+                                              newMaxThetaDot, newXCmdC1, newXCmdC2, \
+                                              newPDThetaC1, newPDThetaC2)
+        self.BUG_FOLLOW = src.LEFT # side of vehicle to wall-follow in bug2
+        self.WALL_FOLLOW_DISTANCE = abs(newWallFollowDist)
+        self.INTERCEPT_DISTANCE = abs(newInterceptDist)
+        self.PF_ZETA = abs(newPFZeta)
+        self.PF_ETA = abs(newPFEta)
+
+
+    def lineTrackingControl(self, line):
+        ''' Computes waypoint orders that will track along a line (the m-line)
+        towards the waypoint.  If the vehicle is beyond the waypoint or closer
+        than the intercept distance, it will drive straight there
+        @param line: directed line that the vehicle is tracking
+        '''
+        if self.waypointDistance < self.INTERCEPT_DISTANCE:
+            self.pdWaypointControl()
+        else:
+            thetaErr = rm.normalize_pi(line.interceptHeading(self.state.pose, \
+                                                             self.INTERCEPT_DISTANCE) - \
+                                       self.state.pose[2])
+            self.setVelocityCommands(thetaErr, self.waypointDistance)
+
+
+    def wallFollowingControl(self, direction):
+        ''' Computes control orders that will drive the vehicle along a wall
+        detected by the laser.  Following will be on the side of the vehicle
+        specified by the argument
+        @param direction: side of the vehicle (left or right) to keep the wall
+        '''
+        followed = []
+        distance = self.laserData.maxRange
+
+        # Update the turn accumulator (reset if we just started)
+        if self.wallFollowCtr <= 10:
+            self.thetaLast = self.state.pose[2]
+            self.turnAccumulator = 0.0
+        else:
+            self.turnAccumulator += \
+                rm.normalize_pi(self.state.pose[2] - self.thetaLast)
+            self.thetaLast = self.state.pose[2]
+
+        # Account for losing track of the wall (i.e., no intervals)
+        if len(self.laserData.intervals) == 0:
+            thetaErr = -direction * math.pi/2.0
+            self.setVelocityCommands(thetaErr, self.waypointDistance)
+            return  # probably a corner--just turn towards the wall
+
+        for interval in self.laserData.intervals:
+            if self.laserData.ranges[interval[2]] < distance:
+                followed = interval
+                distance = self.laserData.ranges[interval[2]]
+
+        # These will be used later
+        trackSegment = 0
+        right = self.laserData.rayReturn(followed[0])
+        left = self.laserData.rayReturn(followed[1])
+        close = self.laserData.rayReturn(followed[2])
+
+        if ((interval[0] == interval[2]) or (interval[1] == interval[2])):
+            if self.BUG_FOLLOW == src.LEFT:
+                trackSegment = src.DirectedLine(left, right)
+            else:
+                trackSegment = src.DirectedLine(right, left)
+        else:
+            if self.BUG_FOLLOW == src.LEFT:
+                trackSegment = src.DirectedLine(close, right)
+            else:
+                trackSegment = src.DirectedLine(close, left)
+
+        trackSegment.rho -= self.WALL_FOLLOW_DISTANCE
+        thetaErr = rm.normalize_pi(trackSegment.interceptHeading((0.0, 0.0), self.INTERCEPT_DISTANCE))
+        self.setVelocityCommands(thetaErr, self.waypointDistance)
+
+
+    def bug2WaypointControl(self):
+        ''' Computes waypoint control orders that will drive the robot
+        towards a commanded waypoint using the Bug2 algorithm
+        '''
+        if not self.waypointCaptured():
+            if (self.navMode == src.LINE_FOLLOWING and \
+                ((self.laserData.obstacleToFront() < (self.WALL_FOLLOW_DISTANCE + 0.5)) or \
+                 ((self.laserData.closestRange < self.WALL_FOLLOW_DISTANCE - 0.25) and \
+                  (rm.signum(self.laserData.closestBearing) == \
+                   rm.signum(rm.normalize_pi(self.mLine.theta - self.state.pose[2])))))):
+                self.navMode = src.WALL_FOLLOWING
+                self.wallFollowCtr = 0
+                self.turnAccumulator = 0.0
+
+            sideOfMLine = rm.signum(self.mLine.distanceFromLine(self.state.pose))
+            if self.navMode == src.WALL_FOLLOWING:
+
+                # If we went in a circle, conclude that we can't get there and move on
+                if abs(self.turnAccumulator) > (2.0 * math.pi):
+                    self.cmd_vel.linear.x = 0.0
+                    self.cmd_vel.angular.z = 0.0
+                    self.isAtWaypoint = True
+                    return
+
+                d = rm.cartesian_distance(self.state.pose, self.waypoint)
+                if self.wallFollowCtr > WALL_FOLLOW_TRANSITION_STEPS:
+                    if ((sideOfMLine != self.sideOfMLine) and \
+                        (d < self.mLine.distance)):
+                        self.navMode = src.LINE_FOLLOWING
+                        self.mLine.updateStart(self.state.pose)
+                elif self.wallFollowCtr == WALL_FOLLOW_TRANSITION_STEPS:
+                    self.mLine.updateStart(self.state.pose)
+                    
+
+            if self.navMode == src.LINE_FOLLOWING:
+                self.lineTrackingControl(self.mLine)
+            else:
+                self.wallFollowingControl(self.BUG_FOLLOW)
+                self.sideOfMLine = sideOfMLine
+                self.wallFollowCtr += 1
+
+
+    def pfWaypointControl(self):
+        ''' Computes waypoint control orders that will drive the robot
+        towards a commanded waypoint using potential field navigation
+        '''
+        if not self.waypointCaptured():
+            # Compute the attractive force to the goal (linear increase with d)
+            hdgTo = rm.normalize_pi(rm.bearing_to(self.state.pose, \
+                                                  self.waypoint) - \
+                                    self.state.pose[2])
+            f = self.PF_ZETA * self.waypointDistance
+            fx = math.cos(hdgTo) * f
+            fy = math.sin(hdgTo) * f
+
+            # Compute the repulsive force for each laser return
+            for rayNum in range(0,  len(self.laserData.ranges)):
+                if self.laserData.ranges[rayNum] > self.laserData.minRange and \
+                   self.laserData.ranges[rayNum] < (self.laserData.maxRange - 0.05):
+                    angle = self.laserData.minAngle + \
+                            self.laserData.increment * rayNum
+                    f = self.PF_ETA * (1.0 / self.laserData.maxRange - \
+                                       1.0 / self.laserData.ranges[rayNum]) * \
+                                      1.0 / (self.laserData.ranges[rayNum] * \
+                                             self.laserData.ranges[rayNum])
+                    fx += math.cos(angle) * f
+                    fy += math.sin(angle) * f
+
+            # Compute actual commands based on computed force vectors
+            thetaErr = rm.normalize_pi(math.atan2(fy, fx))
+            self.setVelocityCommands(thetaErr, self.waypointDistance)
+

project_msgs/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(project_msgs)
+
+## 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
+  rospy
+  roscpp
+  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
+  Circle.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 your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES project_msgs
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/project_msgs.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/project_msgs_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_project_msgs.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)

project_msgs/msg/Circle.msg

+Header header
+float32 x
+float32 y
+float32 radius

project_msgs/package.xml

+<?xml version="1.0"?>
+<package format="2">
+  <name>project_msgs</name>
+  <version>0.0.0</version>
+  <description>The project_msgs package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="cs4313@todo.todo">cs4313</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/project_msgs</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>