ROSBag stuff

.gitignore

@@ -1 +1 @@
-data/
+data/*

CMakeLists.txt

@@ -13,9 +13,20 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   message_filters
   nav_msgs
+  std_msgs
   tf
 )
 
+add_message_files(
+  FILES
+  Float64Stamped.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
 ###################################
 ## catkin specific configuration ##
 ###################################
@@ -28,7 +39,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
    INCLUDE_DIRS include
    LIBRARIES jackal_bsc
-#  CATKIN_DEPENDS geometry_msgs message_filters roscpp rospy
+   CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 

config/exp_params.yaml

@@ -1,3 +1,5 @@
 name: "Bob"
 drift_value: 0.2
 delay_time: 3.0
+# map or odom
+nav_type: "odom"

include/jackal_bsc/BSCSolver.h

@@ -9,6 +9,7 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <nav_msgs/Odometry.h>
 #include <std_msgs/Float64.h>
+#include <jackal_bsc/Float64Stamped.h>
 
 #include <math.h>
 #include <iostream>
@@ -31,10 +32,11 @@ class BSCSolver
     double delta_x, delta_y, dist_to_goal; // Displacement from current location to goal
     double delta_z, user_vel_z, navi_vel;
     double user_vel_x, user_delay_z, user_delay_x;
-    std_msgs::Float64 bsc_param;
+    jackal_bsc::Float64Stamped bsc_param;
     double max_dist;
     double max_vel;
     int q_size;
+    uint32_t seq;
     bool goal_received; // Check if action CB ran
 
     ros::Publisher bsc_pub;

launch/amcl_navigation.launch

@@ -4,6 +4,9 @@
           args="/cmd_vel /jackal_bsc/bsc_vel /jackal_bsc/key_vel /jackal_bsc/nav_vel mux:=bsc_mux">
     </node>
 
+    <!-- Kinect -->
+    <include file="$(find jackal_bsc)/launch/kinect.launch"/>
+
     <!-- Kinect TF -->
     <include file="$(find jackal_bsc)/launch/kinect_tf.launch"/>
 

launch/bag_data.launch

@@ -0,0 +1,4 @@
+<launch>
+    <node pkg="rosbag" type="record" name="rosbag_record_bsc"
+          args="record "/>
+</launch>

launch/experiment/bsc.launch

@@ -1,5 +1,8 @@
 <launch>
+    <rosparam command="load" file="$(find jackal_bsc)/config/exp_params.yaml" ns="jackal_bsc"/>
     <rosparam command="load" file="$(find jackal_bsc)/config/bsc_params.yaml" ns="jackal_bsc"/>
+    <param name="/jackal_bsc/exp_type" type="str" value="BSC"/>
+
     <node name="twist_stamped_publisher" pkg="jackal_bsc" type="twist_stamped_publisher" output="screen"/>
     <node name="bsc_node" pkg="jackal_bsc" type="bsc_node" output="screen"/>
 </launch>

launch/odom_navigation.launch

@@ -4,6 +4,9 @@
           args="/cmd_vel /jackal_bsc/bsc_vel /jackal_bsc/key_vel /jackal_bsc/nav_vel mux:=bsc_mux">
     </node>
 
+    <!-- Kinect -->
+    <include file="$(find jackal_bsc)/launch/kinect.launch"/>
+
     <!-- Kinect TF -->
     <include file="$(find jackal_bsc)/launch/kinect_tf.launch"/>
 

msg/Float64Stamped.msg

@@ -0,0 +1,2 @@
+Header header
+float64 data

package.xml

@@ -53,6 +53,8 @@
   <depend>rospy</depend>
   <depend>geometry_msgs</depend>
   <depend>message_filters</depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
   <depend>tf</depend>
 
   <!-- The export tag contains other, unspecified, tags -->

scripts/data_collection.py

@@ -1,17 +1,18 @@
 #!/usr/bin/env python
 
 import rospy
+import roslaunch
 from rospkg import RosPack
 import actionlib
 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionResult
-from std_msgs.msg import Float64
+from jackal_bsc.msg import Float64Stamped
 import csv
 
 
 class DataCollection:
     def __init__(self):
         # Subscribers
-        rospy.Subscriber('/jackal_bsc/alpha', Float64, self.alpha_cb)
+        rospy.Subscriber('/jackal_bsc/alpha', Float64Stamped, self.alpha_cb)
         rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.goal_cb)
 
         # Clients
@@ -23,6 +24,7 @@ def __init__(self):
         self.drift_value = rospy.get_param("/jackal_bsc/drift_value", 1.0)
         self.delay_time = rospy.get_param("/jackal_bsc/delay_time", 1.0)
         exp_type = rospy.get_param("/jackal_bsc/exp_type", "BSC")
+        nav_type = rospy.get_param("/jackal_bsc/nav_type", "odom")
         self.start_time = 0
         self.end_time = 0
         self.goal_not_reached = True
@@ -39,14 +41,38 @@ def __init__(self):
 
         # Goal
         self.goal = MoveBaseGoal()
-        self.goal.target_pose.header.frame_id = 'map'
-        self.goal.target_pose.pose.position.x = 4.27
-        self.goal.target_pose.pose.position.y = -8.55
-        self.goal.target_pose.pose.orientation.z = 0.01
-        self.goal.target_pose.pose.orientation.w = 0.999
+        self.goal.target_pose.header.frame_id = nav_type
+        self.goal.target_pose.pose.position.x = -11.57
+        self.goal.target_pose.pose.position.y = -15.32
+        self.goal.target_pose.pose.orientation.z = 0.8785
+        self.goal.target_pose.pose.orientation.w = -0.4775
+
+        # Launch Bag file
+        pkg = 'rosbag'
+        type = 'record'
+        bag_compression = '--lz4 '
+        bag_name = '-O ' + pkg_path + '/data/' + user_name + '_' + exp_type + '.bag '
+        topics = '/bluetooth_teleop/joy /cmd_vel /front/scan /imu/data /jackal_bsc/alpha ' \
+                 '/jackal_bsc/bsc_vel /jackal_bsc/key_vel_stamped /jackal_bsc/nav_vel_stamped /jackal_laser_scan ' \
+                 '/kinect2/qhd/camera_info /kinect2/qhd/image_color_rect ' \
+                 '/kinect2/qhd/image_depth_rect /kinect2/qhd/points ' \
+                 '/move_base/NavfnROS/plan /move_base/TrajectoryPlannerROS/cost_cloud ' \
+                 '/move_base/TrajectoryPlannerROS/global_plan /move_base/TrajectoryPlannerROS/local_plan ' \
+                 '/move_base/cancel /move_base/current_goal /move_base/feedback /move_base/global_costmap/costmap ' \
+                 '/move_base/global_costmap/costmap_updates /move_base/global_costmap/footprint /move_base/goal' \
+                 '/move_base/local_costmap/costmap /move_base/local_costmap/costmap_updates ' \
+                 '/move_base/local_costmap/footprint ' \
+                 '/move_base/local_costmap/obstacles_layer_footprint/footprint_stamped /move_base/result ' \
+                 '/move_base/status /move_base_simple/goal /odometry/filtered /tf /tf_static '
+        self.node = roslaunch.core.Node(pkg, type, args=bag_compression + bag_name + topics)
+        self.launch = roslaunch.scriptapi.ROSLaunch()
+        self.launch.start()
+
+        rospy.loginfo("\n[Data]: Name: {}\nExperiment Type: {}\n"
+                      "Drift Value: {}\nDelay Value: {}".format(user_name, exp_type, self.drift_value, self.delay_time))
 
     def alpha_cb(self, msg):
-        self.alpha_writer.writerow([msg.data, rospy.get_time()])
+        self.alpha_writer.writerow([msg.data, msg.header.stamp])
 
     def goal_cb(self, msg):
         if msg.status.status == actionlib.GoalStatus.SUCCEEDED:
@@ -55,25 +81,34 @@ def goal_cb(self, msg):
             self.end()
 
     def start(self):
-        rospy.loginfo("[Data]: Experiment started")
+        rospy.loginfo("[Data]: Launching bag")
+        self.roslaunch_process = self.launch.launch(self.node)
+        rospy.loginfo("[Data]: Bagging: {}".format(self.roslaunch_process.is_alive()))
         try:
-            self.start_time = rospy.Time().now()
             self.move_client.send_goal(self.goal)
+            self.start_time = rospy.Time().now()
+            rospy.loginfo("[Data]: Start experiment")
             rospy.spin()
         except KeyboardInterrupt:
-            rospy.loginfo("[Data]: User stopped! Ending data collection.")
+            rospy.logwarn("[Data]: User stopped! Ending data collection.")
             self.end()
 
     def end(self):
         rospy.loginfo("[Data]: Completing data collection...")
+        rospy.signal_shutdown("End experiment")
+        self.roslaunch_process.stop()
         self.end_time = rospy.Time().now()
         total_time = self.end_time - self.start_time
         format_time = str(total_time.secs) + '.' + str(total_time.nsecs)
+        self.time_writer.writerow(["Start Time:", self.start_time])
+        self.time_writer.writerow(["End Time:", self.end_time])
         self.time_writer.writerow(["Time:", format_time])
         self.time_writer.writerow(["Goal Reached:", self.goal_not_reached])
+        rospy.loginfo("[Data]: Time: {}".format(format_time))
+        rospy.loginfo("[Data]: Goal Not Reached: {}".format(self.goal_not_reached))
         self.alpha_csv.close()
         self.time_csv.close()
-        exit()
+        exit(0)
 
     def __del__(self):
         self.end()

src/BSCSolver.cpp

@@ -24,10 +24,11 @@ BSCSolver::BSCSolver(ros::NodeHandle* nh):
     max_dist = 15.0;
     max_vel = 3.0;
     q_size = 10;
+    seq = 0;
     goal_received = false;
     // Create publishers
     bsc_pub = n.advertise<Twist>("/jackal_bsc/bsc_vel", q_size);
-    alpha_pub = n.advertise<std_msgs::Float64>("/jackal_bsc/alpha", q_size);
+    alpha_pub = n.advertise<jackal_bsc::Float64Stamped>("/jackal_bsc/alpha", q_size);
 
     // Create normal subscribers
     goal_sub = n.subscribe(goal_topic, 10, &BSCSolver::goal_cb, this);
@@ -56,7 +57,10 @@ void BSCSolver::bsc_cb(const geometry_msgs::TwistStampedConstPtr &nav_vel,
                        const geometry_msgs::TwistStampedConstPtr &key_vel,
                        const nav_msgs::OdometryConstPtr& odom)
 {
+    ROS_WARN_THROTTLE(15, "[BSC]: No goal received...");
     if (goal_received) {
+        ROS_INFO_THROTTLE(10, "[BSC]: Blending...");
+        seq++;
         // Compute difference in user and navigation commands
         user_vel_z = key_vel->twist.angular.z;
         user_vel_x = key_vel->twist.linear.x;
@@ -71,6 +75,8 @@ void BSCSolver::bsc_cb(const geometry_msgs::TwistStampedConstPtr &nav_vel,
         dist_to_goal = hypot(delta_x, delta_y);
 
         // Compute BSC parameter
+        bsc_param.header.seq = seq;
+        bsc_param.header.stamp = ros::Time::now();
         bsc_param.data = fmax(0, (1 - (dist_to_goal / max_dist))) *
                     fmax(0, (1 - pow(delta_z / max_vel, 2)));
         alpha_pub.publish(bsc_param);