Added rover catkin packages

rover/drive_test/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(drive_test)
+
+## 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
+  stage_ros
+  geometry_msgs
+  roscpp
+  rospy
+)
+
+catkin_package()
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

rover/drive_test/launch/drive_test.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="stage_ros" type="stageros" name="simulator" args="$(find drive_test)/worlds/manyDots.world"/>
+  <node pkg="drive_test" type="drive_test.py" name="hw2" output="screen">
+	  <param name="goalX" value="20" />
+    <param name="goalY" value="-20" />
+  </node>
+</launch>

rover/drive_test/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package>
+  <name>drive_test</name>
+  <version>0.0.0</version>
+  <description>ROB456 HW2</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="rob456@todo.todo">rob456</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>BSD</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/pioneer_delivery</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>geometry_msgs</build_depend>
+  <build_depend>stage_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>stage_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

rover/drive_test/src/drive_test.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python
+
+import rospy
+import math
+import tf
+from tf.transformations import euler_from_quaternion
+import message_filters
+from pprint import pprint
+import time
+# The laser scan message
+from sensor_msgs.msg import Joy
+
+# The odometry message
+from nav_msgs.msg import Odometry
+
+# the velocity command message
+from geometry_msgs.msg import Twist
+
+# global pi - this may come in handy
+pi = math.pi
+
+# method to control the robot
+def callback_spacenav(twist_message):
+    scalar_forward = 100.0
+    scalar_turn = 2.0
+
+    # make a new twist message
+    command = Twist()
+
+    # Fill in the fields.  Field values are unspecified 
+    # until they are actually assigned. The Twist message 
+    # holds linear and angular velocities.
+    command.linear.x = twist_message.linear.x * scalar_forward
+    command.linear.y = twist_message.linear.y * scalar_forward
+    command.linear.z = twist_message.linear.z * scalar_forward
+    command.angular.x = twist_message.angular.x * scalar_forward
+    command.angular.y = twist_message.angular.y * scalar_forward
+    command.angular.z = twist_message.angular.z * scalar_turn
+
+    # print command
+
+    # Send the commands
+    pub.publish(command)
+
+# method to control the robot
+def callback_joy(joy_message):
+    scalar_forward = 100.0
+    scalar_turn = 2.0
+
+    # make a new twist message
+    command = Twist()
+
+    # Fill in the fields.  Field values are unspecified 
+    # until they are actually assigned. The Twist message 
+    # holds linear and angular velocities.
+    command.linear.x = joy_message.axes[1] * 100
+    #command.linear.y = twist_message.linear.y * scalar_forward
+    #command.linear.z = twist_message.linear.z * scalar_forward
+    #command.angular.x = twist_message.angular.x * scalar_forward
+    #command.angular.y = twist_message.angular.y * scalar_forward
+    command.angular.z = joy_message.axes[0] * 1.0
+
+    # command.angular.z = joy_message.axes[2] * 1.0
+
+    print joy_message
+
+    # Send the commands
+    pub.publish(command)
+
+# main function call
+if __name__ == "__main__":
+    # Initialize the node
+    rospy.init_node('drive_test', log_level=rospy.DEBUG)
+
+    # subscribe to laser scan message
+    # sub = rospy.Subscriber('/spacenav/twist', Twist, callback_spacenav)
+
+    # subscribe to laser scan message
+    sub = rospy.Subscriber('/joy', Joy, callback_joy)
+
+    # synchronize laser scan and odometry data
+    # ts = message_filters.TimeSynchronizer([sub, sub2], 10)
+    # ts.registerCallback(callback)
+
+    # publish twist message
+    pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
+
+    # Turn control over to ROS
+    rospy.spin()
+

rover/drive_test/worlds/manyDots.world

@@ -0,0 +1,71 @@
+define block model
+(
+  size [0.5 0.5 0.5]
+  gui_nose 0
+)
+
+define topurg ranger
+(
+	sensor( 			
+    range [ 0.0  30.0 ]
+    fov 270.25
+   samples 1081
+  )
+
+  # generic model properties
+  color "black"
+  size [ 0.05 0.05 0.1 ]
+)
+
+define erratic position
+(
+  size [0.35 0.35 0.25]
+  origin [-0.05 0 0 0]
+  gui_nose 0
+  drive "diff"
+  topurg(pose [ 0.050 0.000 0 0.000 ])
+  odom_error [0.00 0.00 0.00 0.00 0.00 0.00]
+)
+
+define floorplan model
+(
+  # sombre, sensible, artistic
+  color "gray30"
+
+  # most maps will need a bounding box
+  boundary 1
+
+  gui_nose 0
+  gui_grid 0
+
+  gui_outline 0
+  gripper_return 0
+  fiducial_return 0
+  laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.005
+
+interval_sim 100  # simulation timestep in milliseconds
+
+
+window
+( 
+  size [ 745.000 448.000 ] 
+
+  rotate [ 0.000 0.000 ]
+  scale 5
+)
+
+# load an environment bitmap
+floorplan
+( 
+  name "manyDots"
+  bitmap "manyDots.pgm"
+  size [54.0 58.7 0.5]
+  pose [ 0 0.0 0 90.000 ]
+)
+
+# throw in a robot
+erratic( pose [ -10.000 10.000 0 90.000 ] name "era" color "blue" localizaion "gps" localization_origin [0 0 0 0])