0.2.4 (#47)

* 0.2.4

README.md

@@ -63,10 +63,15 @@ Please submit pull requests to the [devel branch](https://github.com/AutoRally/a
 
 Get the autorally repository in a [catkin workspace](http://wiki.ros.org/catkin/workspaces). The suggested location is `~/catkin_ws/src/`, but any valid catkin worskspace source folder will work. We suggest forking first if you will be working with the code.
 
-To clone straight from the AutoRally:
+
+To clone straight from the AutoRally repo:
 
     git clone https://github.com/AutoRally/autorally.git
 
+Also clone the IMU code into the same catkin workspace:
+
+    git clone https://github.com/AutoRally/imu_3dm_gx4.git
+
 ### 3. Install AutoRally ROS Dependencies
 
 Within the catkin workspace folder, run this command to install the packages this project depends on.
@@ -83,7 +88,7 @@ Due to the additional requirement of ROS's distributed launch system, you must r
 
 before using any AutoRally components. See the [wiki](https://github.com/AutoRally/autorally/wiki) for more information about how to set this system up for distributed launches on your vehicle platform.
 
-_Note:_ If you are unfamiliar with catkin, please know that you must run `source <catkin_ws>/devel/setup.sh` before ROS will be able to locate the autorally packages. This line can be added to your ~/.bashrc file.
+_Note:_ If you are unfamiliar with catkin, please know that you must run `source catkin_ws/devel/setup.sh` before ROS will be able to locate the autorally packages. This line can be added to your ~/.bashrc file.
 
 ### 5. Generate Documentation
 

autorally/CHANGELOG.rst

@@ -0,0 +1,18 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autorally
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.4 (2017-02-14)
+------------------
+
+0.2.3 (2016-09-20 11:06)
+------------------------
+
+0.2.2 (2016-09-20 11:05:39)
+---------------------------
+
+0.2.1 (2016-09-20 11:05:16)
+---------------------------
+
+0.2.0 (2016-09-20 11:04)
+------------------------

autorally/package.xml

@@ -16,7 +16,7 @@
 -->
 <package>
   <name>autorally</name>
-  <version>0.2.3</version>
+  <version>0.2.4</version>
   <description>Autorally metapackage</description>
 
   <author>Brian Goldfain</author>

autorally_control/CHANGELOG.rst

@@ -0,0 +1,18 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autorally_control
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.4 (2017-02-14)
+------------------
+
+0.2.3 (2016-09-20 11:06)
+------------------------
+
+0.2.2 (2016-09-20 11:05:39)
+---------------------------
+
+0.2.1 (2016-09-20 11:05:16)
+---------------------------
+
+0.2.0 (2016-09-20 11:04)
+------------------------

autorally_control/package.xml

@@ -16,7 +16,7 @@
 -->
 <package>
   <name>autorally_control</name>
-  <version>0.2.3</version>
+  <version>0.2.4</version>
   <description>All control software created for the autorally project resides here</description>
 
   <maintainer email="bgoldfain3@gatech.edu">Brian Goldfain</maintainer>

autorally_core/CHANGELOG.rst

@@ -0,0 +1,27 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package autorally_core
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.2.4 (2017-02-14)
+------------------
+* New camera_trigger Arduino and .cpp code
+* removed GPGSV messages from being displayed in diagnostics
+* SystemStatus: added disc usage, removed m4api stats that are causing a hang on startup, fixed broken fan speed parsing
+* Xbee: rewrite to send GPS RTK corrections better, allow multiple coordinators, transmit/receive state estimates to/from other vehicles
+* Improved error checking/handling across the board
+* AutorallyChassis: streamlined non-critical error handling
+* Fixed incorrect wheel speeds published by autorally_chassis by adding conversion from rotations persecond to m/s
+* ChronyStatus diagnostic topic is now color coded to idicate whether it is synchonizing to a clock (green) or not (yellow)
+* Added wheelOdometry node
+
+0.2.3 (2016-09-20 11:06)
+------------------------
+
+0.2.2 (2016-09-20 11:05:39)
+---------------------------
+
+0.2.1 (2016-09-20 11:05:16)
+---------------------------
+
+0.2.0 (2016-09-20 11:04)
+------------------------

autorally_core/CMakeLists.txt

@@ -21,7 +21,10 @@ find_package(cmake_modules REQUIRED)
 
 catkin_python_setup()
 
-generate_dynamic_reconfigure_options(cfg/camera_auto_balance_params.cfg)
+generate_dynamic_reconfigure_options(
+  cfg/camera_auto_balance_params.cfg
+  cfg/camera_trigger_params.cfg
+)
 
 catkin_package(
     DEPENDS libqt4-dev lm-sensors Boost
@@ -49,7 +52,9 @@ add_subdirectory(src/xbee)
 add_subdirectory(src/ImageRepublisher)
 add_subdirectory(src/StateEstimator)
 add_subdirectory(src/autorally_chassis)
+add_subdirectory(src/camera_trigger)
 add_subdirectory(src/CameraAutoBalance)
+add_subdirectory(src/WheelOdometry)
 
 #install(TARGETS
 # 

autorally_core/cfg/camera_trigger_params.cfg

@@ -0,0 +1,13 @@
+#! /usr/bin/env python
+
+PACKAGE='autorally_core'
+import roslib
+roslib.load_manifest(PACKAGE)
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+#       Name       Type      Level Description     Default Min   Max
+gen.add("camera_trigger_frequency",     int_t,    0,    "FPS for camera triggering",      40,    0, 150)
+
+exit(gen.generate(PACKAGE, "autorally_core", "camera_trigger_params"))

autorally_core/launch/autorally.launch

@@ -6,6 +6,7 @@
 
   <include file="$(find autorally_core)/launch/ocs.launch" />
   <include file="$(find autorally_core)/launch/autorally_core_manager.launch" />
+  <include file="$(find autorally_core)/launch/wheel_odometry.launch" />
 
   <!-- depending on if we're using an old or new chassis, launch the appropriate interface -->
   <group if="$(optenv AR_CHASSIS_NEW true)">
@@ -24,5 +25,4 @@
     <include file="$(find autorally_core)/launch/chronyStatus.launch" />
   </group>
 
-
 </launch>

autorally_core/launch/autorally_chassis.launch

@@ -6,6 +6,7 @@
     <param name="commandRate" value="75" />
     <param name="commandMaxAge" value="0.2" />
     <param name="runstopMaxAge" value="2" />
+    <param name="wheelDiameter" value="0.190" />
 
     <rosparam param="actuators" command="load" file="$(env AR_CONFIG_PATH)/arChassisConfig_$(env AR_CHASSIS).yaml" />
 

autorally_core/launch/autorally_imu_3dm_gx4.launch

@@ -7,7 +7,7 @@
         <param name="baudrate" type="int" value="921600" />
         <param name="frame_id" type="string" value="imu"/>
         <param name="imu_rate" type="int" value="200" />
-        <param name="filter_rate" type="int" value="100"/>
+        <param name="filter_rate" type="int" value="200"/>
         <param name="enable_filter" type="bool" value="true"/>
         <param name="enable_accel_update" type="bool" value="true"/>
         <param name="enable_mag_update" type="bool" value="false"/>

autorally_core/launch/camera_trigger.launch

@@ -0,0 +1,16 @@
+<launch>
+  <include file="$(find autorally_core)/launch/hardware.machine" />
+
+  <node pkg="nodelet" type="nodelet" name="CameraTrigger" args="load autorally_core/CameraTrigger camera_nodelet_manager" output="screen" machine="autorally-master" >
+
+    <param name="port" value="/dev/arComputeBoxArduino" />
+
+    <!--configure settings for 115200 baud, 8N1 -->
+    <param name="serialBaud" value="115200" />
+    <param name="serialDataBits" value="8" />
+    <param name="serialParity" value="none" />
+    <param name="serialStopBits" value="1" />
+    <param name="serialHardwareFlow" value="false" />
+    <param name="serialSoftwareFlow" value="false" />
+  </node>
+</launch>

autorally_core/launch/cameras.launch

@@ -5,6 +5,10 @@
 
   <node pkg="nodelet" type="nodelet" name="camera_nodelet_manager" machine="autorally-master" output="screen" args="manager" />
 
+  <group if="$(optenv AR_CHASSIS_NEW true)">
+    <include file="$(find autorally_core)/launch/camera_trigger.launch"/>
+  </group>
+
   <include file="$(find autorally_core)/launch/camera.launch" if="$(env AR_LEFTCAM_CONNECTED)">
     <arg name="nodelet_manager_launched" value="1" />
     <arg name="serial" value="$(optenv AR_LEFTCAM 'none')" />

autorally_core/launch/gpsRover.launch

@@ -10,7 +10,7 @@
     <param name="accuracyAutonomous" value="2.5" />
     <param name="utcSource" value="GPZDA" />
     <param name="statusPositionSource" value="GPGNS" /> <!-- GPGNS | GPGGA -->
-    <param name="showGsv" value="true" />
+    <param name="showGsv" value="false" />
 
     <!--configure settings for primary port -->
     <param name="primaryPort/portPath" value="/dev/arGPSroverPortA" />

autorally_core/launch/runStop.launch

@@ -4,7 +4,8 @@
   <node name="runStop" pkg="autorally_core" type="runStop"
         machine="autorally-ocs" output="screen">
     <remap from="runstop" to="runstopBox"/>
-
+
+    <param name="runstopRate" value="5"/>
     <param name="port" value="/dev/arRunStop" />
 
     <!--configure settings for 57600 baud, 8N1 -->

autorally_core/launch/stateEstimator.launch

@@ -37,7 +37,7 @@
       <!-- To use in sumulation set "rosparam set /gps_imu/FixedInitialPose true" -->
       <!-- and <param name="InvertY" value="false"/> -->
       <!-- and <param name="InvertZ" value="false"/> -->
-      <param name="FixedInitialPose" value="true"/>
+      <param name="FixedInitialPose" value="false"/>
       <param name="initialRoll" value="0"/>
       <param name="intialPitch" value="0"/>
       <param name="initialYaw" value="0"/>

autorally_core/launch/systemStatus.launch

@@ -7,5 +7,8 @@
     <param name="batteryCrit" value="20" />
     <param name="wirelessLow" value="0" />
     <param name="wirelessCrit" value="0" />
+    <param name="dataDrive" value="/media/data" />
+    <param name="dataDriveSpaceHighPercent" value="75" />
+    <param name="dataDriveSpaceCritPercent" value="90" />
   </node>
 </launch>

autorally_core/launch/wheel_odometry.launch

@@ -0,0 +1,13 @@
+<launch>
+  <node name="WheelOdometry" pkg="autorally_core" type="WheelOdometry" output="screen"/>
+    <param name="vehicle_wheelbase" value="0.57785" />
+    <param name="vehicle_width" value="0.3175" />
+    <param name="using_sim" value="false" />
+
+    <!-- note: the debug parameter delays the output of the angular velocity -->
+    <param name="debug" value="false" />
+
+    <!-- time_delay applies to the angular velocity calculation that lines data up with state estimator - only debug mode -->
+    <param name="time_delay" value="0.2714" />
+
+</launch>

autorally_core/launch/xbeeCoordinator.launch

@@ -15,7 +15,7 @@
     <param name="serialDataBits" value="8" />
     <param name="serialParity" value="none" />
     <param name="serialStopBits" value="1" />
-    <param name="serialHardwareFlow" value="false" />
+    <param name="serialHardwareFlow" value="true" />
     <param name="serialSoftwareFlow" value="false" />
   </node>
 </launch>

autorally_core/launch/xbeeNode.launch

@@ -14,7 +14,7 @@
     <param name="serialDataBits" value="8" />
     <param name="serialParity" value="none" />
     <param name="serialStopBits" value="1" />
-    <param name="serialHardwareFlow" value="false" />
+    <param name="serialHardwareFlow" value="true" />
     <param name="serialSoftwareFlow" value="false" />
   </node>
 </launch>

autorally_core/nodelet_plugins.xml

@@ -34,3 +34,11 @@
     </description>
   </class>
 </library>
+
+<library path="lib/libCameraTrigger">
+  <class name="autorally_core/CameraTrigger" type="autorally_core::CameraTrigger" base_class_type="nodelet::Nodelet">
+    <description>
+    Camera Trigger nodelet
+    </description>
+  </class>
+</library>

autorally_core/package.xml

@@ -16,7 +16,7 @@
 -->
 <package>
   <name>autorally_core</name>
-  <version>0.2.3</version>
+  <version>0.2.4</version>
   <description>All core software (code specific to hardware on the robot) created for the autorally project resides here</description>
 
   <maintainer email="bgoldfain3@gatech.edu">Brian Goldfain</maintainer>

autorally_core/src/RunStop/RunStop.cpp

@@ -48,26 +48,34 @@ int main(int argc, char **argv)
 	}
 
 	RunStop runStop(nh, runStopPort);
-	runStop.runstopPub_ = nh.advertise<autorally_msgs::runstop>("runstop", 1);
-
-  //publish runsto pat 10Hz into system
-	runStop.doWorkTimer_ = nh.createTimer(ros::Rate(10),
-	                                     &RunStop::doWorkTimerCallback,
-	                                     &runStop);
-
+
 	ros::spin();
 	return 0;
 }
 
 RunStop::RunStop(ros::NodeHandle &nh,
-                             const std::string& port):
-  state_("RED")
+                 const std::string& port):
+                 state_("RED")
 {
+  lastMessageTime_ = ros::Time::now() + ros::Duration(5.0);
+
+  double runstopRate = 5.0;
+  if(!nh.getParam("runStop/runstopRate", runstopRate))
+	{
+		ROS_ERROR("Could not get all RunStop parameters");
+	}
+
+	runstopPub_ = nh.advertise<autorally_msgs::runstop>("runstop", 1);
+
   serialPort_.init(nh, ros::this_node::getName(), "","RunStop", port, true);
+  //publish runsto pat 10Hz into system
+	doWorkTimer_ = nh.createTimer(ros::Rate(runstopRate),
+                                &RunStop::doWorkTimerCallback,
+                                this);
+
+
   runstopData_.header.frame_id="RUNSTOP";
   runstopData_.sender = "RUNSTOP";
-
-  lastMessageTime_ = ros::Time::now() + ros::Duration(1);
 }
 
 RunStop::~RunStop()
@@ -128,7 +136,7 @@ void RunStop::doWorkTimerCallback(const ros::TimerEvent& /*time*/)
   }
 
   //if no recent message, runstop is false
- 	if((ros::Time::now()-lastMessageTime_).toSec() > 1.0)
+ 	if( ros::Time::now()-lastMessageTime_ > ros::Duration(1.0))
  	{
  	  serialPort_.diag_error("No recent data from runstop box");
  	  runstopData_.motionEnabled = false;

autorally_core/src/SerialSensorInterface/SerialInterfaceThreaded.cpp

@@ -170,7 +170,14 @@ void SerialInterfaceThreaded::run()
         //callback triggered within same thread
         if(m_dataCallback)
         {
-          m_dataCallback();
+          try
+          {
+            m_dataCallback();
+          } catch(boost::bad_function_call &)
+          {
+            //catch this exception for cleaner shutdown
+            std::cout << "Caught bad function call in SerialInterfaceThreaded (probably during shutdown)" << std::endl;
+          }
         }
         //condition can notify (wake) other threads waiting for data
 //        m_waitCond.notify_all();

autorally_core/src/WheelOdometry/CMakeLists.txt

@@ -0,0 +1,10 @@
+add_executable(WheelOdometry wheel_odometry.cpp)
+target_link_libraries(WheelOdometry ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+add_dependencies(WheelOdometry autorally_msgs_gencpp)
+
+install(TARGETS
+    WheelOdometry
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)