Robosub 2018

build.sh

@@ -16,8 +16,7 @@ function help() {
     echo ""
     echo "Targets:"
     echo "  host (Default)                  Builds code to be run on host into build/host/"
-    echo "  jetson                          Builds code to be run on jetson into build/jetson/"
-    echo "  download                        Builds code to be run on the jetson and uploads the code to the jetson"
+    echo "  download                        Downloads the code to the jetson"
     echo "  clean                           Cleans build folders in build/"
     echo "  clang-format                    Runs clang format on all C++ code"
     echo ""
@@ -33,15 +32,16 @@ fi
 
 
 DOWNLOAD=
-TARGET=${1:-host}
-if [ "$TARGET" == "download" ]; then
-    DOWNLOAD=Y
-    TARGET=jetson
+TARGET=host
+if [ "$1" == "download" ]; then
+    ssh [email protected] chmod -R u+w /home/ubuntu/catkin_ws/src/robosub
+    rsync -r --delete --exclude .git --update $ROOT_DIR [email protected]:~/catkin_ws/src/
+    ssh [email protected] chmod -R aug-w /home/ubuntu/catkin_ws/src/robosub
+    exit 0
 fi
 
 case $TARGET in
     host);;
-    jetson);;
     clang-format)
         docker run -it -v $(pwd):/catkin_ws/src/robosub -w /catkin_ws/ \
             illinoisauv/robosub:latest catkin_make --make-args clang-format
@@ -71,6 +71,3 @@ $ROOT_DIR/tools/docker/$TARGET/run.sh ${@:2}
 popd
 
 
-if [ ! -z "$DOWNLOAD" ]; then
-    rsync -r --delete --update $ROOT_DIR/build/jetson/ [email protected]:~/catkin_ws/
-fi

launch/rosbag.launch

@@ -0,0 +1,15 @@
+<launch>
+
+  <arg name="topics" default="
+    /zed/left/image_rect_color
+    /zed/right/image_rect_color
+    /zed/odom
+    /zed/disparity/disparity_image
+    /zed/depth/depth_registered
+    /zed/map
+    /zed/imu/data" />
+  <node pkg="rosbag" name="record" type="record" args="-o /home/ubuntu/bag --bz2
+    $(arg topics)" />
+
+
+</launch>

launch/zed.launch

@@ -1,16 +1,26 @@
 <launch>
-    <!-- Odometry coordinate frame -->
-    <arg name="odometry_frame"       default="map" />
-    <arg name="camera_model"          default="0" /> <!-- 0=ZED, 1=ZEDM-->
-    <arg name="serial_number"           default="0" /> 
+    <arg name="svo_file"              default="" />
+
+    <arg name="camera_model"         default="0" />
+    <arg name="verbose"              default="true" />
+    <arg name="resolution"           default="2" />
+    <arg name="frame_rate"           default="30" />
+    <arg name="serial_number"        default="0" />
+
+    <!-- Coordinate frame -->
+    <arg name="pose_frame"           default="map" />
+    <arg name="odometry_frame"       default="odom" />
 
     <group ns="zed">
         <include file="$(find zed_wrapper)/launch/zed_camera.launch">
-            <!-- compliant mode for rviz -->
-            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+            <arg name="svo_file"            value="$(arg svo_file)" />
+            <arg name="pose_frame"          value="$(arg pose_frame)" />
+            <arg name="odometry_frame"      value="$(arg odometry_frame)" />
             <arg name="camera_model"        value="$(arg camera_model)" />
-            <arg name="serial_number"           default="$(arg serial_number)" /> 
+            <arg name="serial_number"       default="$(arg serial_number)" />
+            <arg name="resolution"          default="$(arg resolution)" />
+            <arg name="frame_rate"          default="$(arg frame_rate)" />
+            <arg name="verbose"             default="$(arg verbose)" />
         </include>
     </group>
-
 </launch>

src/experimental/extract_imu_time.py

@@ -0,0 +1,21 @@
+import tf.transformations as tf_transform
+import time
+import rospy
+
+from sensor_msgs.msg import Imu
+
+i = 0
+start_time = time.time()
+
+def callback(msg):
+    if i % 1000 == 0:
+        euler = tf_transform.euler_from_quaternion(msg.data.orientation)
+        print('Time: %f'.format(time.time() - start_time))
+        print('IMU: Roll(%f) Pitch(%f) Yaw(%f)'.format(euler[0], euler[1], euler[2]))
+
+def main():
+    imu_sub = rospy.Subscriber('/mavros/imu/data', Imu, callback)
+    rospy.spin()
+
+if _name_ == '_main_':
+    main()

src/motion/rcmavros.cpp

@@ -2,8 +2,8 @@
 #include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/OverrideRCIn.h>
 #include <mavros_msgs/SetMode.h>
-#include <tf/tf.h>
 #include <string>
+#include <tf/tf.h>
 
 #include <unistd.h>
 
@@ -24,7 +24,7 @@ const std::string kModeService = "/mavros/set_mode";
 const std::string kArmingService = "/mavros/cmd/arming";
 
 MavrosRCController::MavrosRCController() {
-  usleep(2 * 1000000);  // 2 sec sleep
+  usleep(5 * 1000000); // 2 sec sleep
 
   // Wait for the mavros node to come up
   if (!ros::service::waitForService(kModeService)) {
@@ -72,9 +72,15 @@ uint16_t MavrosRCController::angleToPpm(double angle) {
 
 // Convert a linear speed (0.0-1.0) to RC signal
 uint16_t MavrosRCController::speedToPpm(double speed) {
-  if (speed > 1.0 || speed < -1.0) {
-    ROS_ERROR("Invalid speed requested: %f", speed);
-    return 1500;
+  // if (speed > 1.0 || speed < -1.0) {
+  //   ROS_ERROR("Invalid speed requested: %f", speed);
+  //   return 1500;
+  // }
+  if (speed > 1.0) {
+    speed = 1.0;
+  }
+  if (speed < -1.0) {
+    speed = -1.0;
   }
   return 1500 + speed * 500.0;
 }
@@ -97,14 +103,13 @@ void MavrosRCController::DoUpdate() {
   setpoint_pos_.pose.orientation = QuaternionRPY(roll, pitch, yaw);
 
   // Send target message to ArduPilot
-  msg.channels[1] = angleToPpm(roll);
   msg.channels[0] = angleToPpm(pitch);
-  msg.channels[3] = speedToPpm(setpoint_vel_.twist.angular.z);
-
-  msg.channels[5] = speedToPpm(setpoint_vel_.twist.linear.x);
-  msg.channels[6] = speedToPpm(setpoint_vel_.twist.linear.y);
+  msg.channels[1] = angleToPpm(roll);
   msg.channels[2] = speedToPpm(setpoint_vel_.twist.linear.z);
+  msg.channels[3] = speedToPpm(setpoint_vel_.twist.angular.z);
+  msg.channels[4] = speedToPpm(setpoint_vel_.twist.linear.x);
+  msg.channels[5] = speedToPpm(setpoint_vel_.twist.linear.y);
 
-  msg.channels[4] = 1500;
+  msg.channels[6] = 1500;
   rc_pub_.publish(msg);
 }

src/task_manager/accel_graph.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import roslib
+import sys
+import rospy
+import numpy as np
+import matplotlib.pyplot as plt
+
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Float32
+
+class accel_graph:
+    def __init__(self):
+        self.first = False
+        self.prev_time = 0
+        self.prev_accel = 0
+        self.count = 0
+        rospy.init_node('accel_graph', anonymous=True)
+        self.sub = rospy.Subscriber("/rexrov/imu", Imu, self.Imucallback)
+        self.pub = rospy.Publisher("jerk", Float32 , queue_size = 10)
+
+    def Imucallback(self,data):
+        if not self.first:
+            self.prev_accel = float(data.linear_acceleration.x)
+            self.prev_time = float(data.header.stamp.nsecs)
+            self.first = True
+            self.count += 1
+        else:
+            curr_accel = float(data.linear_acceleration.x)
+            curr_time = float(data.header.stamp.nsecs)
+            self.count += 1
+
+            dt =  abs(curr_time - self.prev_time)/1e7
+            daccel = curr_accel - self.prev_accel
+            jerk = daccel/dt
+
+            self.prev_accel = curr_accel
+            self.prev_time = curr_time
+
+            msg = Float32()
+            msg.data = jerk
+            self.pub.publish(msg)
+
+def main(args):
+    a_g = accel_graph()
+
+    rospy.spin()
+
+if __name__ == '__main__':
+    main(sys.argv)