From 1030ffed7193f4a0486f196942df15c90d819d12 Mon Sep 17 00:00:00 2001 From: Anas Abou Allaban Date: Tue, 24 Jul 2018 12:02:39 -0400 Subject: [PATCH] ROSBag stuff --- .gitignore | 2 +- CMakeLists.txt | 13 +++++++- config/exp_params.yaml | 2 ++ include/jackal_bsc/BSCSolver.h | 4 ++- launch/amcl_navigation.launch | 3 ++ launch/bag_data.launch | 4 +++ launch/experiment/bsc.launch | 3 ++ launch/odom_navigation.launch | 3 ++ msg/Float64Stamped.msg | 2 ++ package.xml | 2 ++ scripts/data_collection.py | 59 +++++++++++++++++++++++++++------- src/BSCSolver.cpp | 8 ++++- 12 files changed, 89 insertions(+), 16 deletions(-) create mode 100644 launch/bag_data.launch create mode 100644 msg/Float64Stamped.msg diff --git a/.gitignore b/.gitignore index adbb97d..60baa9c 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1 @@ -data/ \ No newline at end of file +data/* diff --git a/CMakeLists.txt b/CMakeLists.txt index a4163a3..2898849 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/config/exp_params.yaml b/config/exp_params.yaml index aa418c5..7fc0d45 100644 --- a/config/exp_params.yaml +++ b/config/exp_params.yaml @@ -1,3 +1,5 @@ name: "Bob" drift_value: 0.2 delay_time: 3.0 +# map or odom +nav_type: "odom" \ No newline at end of file diff --git a/include/jackal_bsc/BSCSolver.h b/include/jackal_bsc/BSCSolver.h index 69862e9..e4aa398 100644 --- a/include/jackal_bsc/BSCSolver.h +++ b/include/jackal_bsc/BSCSolver.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -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; diff --git a/launch/amcl_navigation.launch b/launch/amcl_navigation.launch index 9cbb281..a0dc827 100644 --- a/launch/amcl_navigation.launch +++ b/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"> + + + diff --git a/launch/bag_data.launch b/launch/bag_data.launch new file mode 100644 index 0000000..d47c5e3 --- /dev/null +++ b/launch/bag_data.launch @@ -0,0 +1,4 @@ + + + \ No newline at end of file diff --git a/launch/experiment/bsc.launch b/launch/experiment/bsc.launch index cee61a5..081824c 100644 --- a/launch/experiment/bsc.launch +++ b/launch/experiment/bsc.launch @@ -1,5 +1,8 @@ + + + \ No newline at end of file diff --git a/launch/odom_navigation.launch b/launch/odom_navigation.launch index 56d9827..729c14c 100644 --- a/launch/odom_navigation.launch +++ b/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"> + + + diff --git a/msg/Float64Stamped.msg b/msg/Float64Stamped.msg new file mode 100644 index 0000000..5c2f113 --- /dev/null +++ b/msg/Float64Stamped.msg @@ -0,0 +1,2 @@ +Header header +float64 data diff --git a/package.xml b/package.xml index 2d0e758..d443551 100644 --- a/package.xml +++ b/package.xml @@ -53,6 +53,8 @@ rospy geometry_msgs message_filters + message_generation + message_runtime tf diff --git a/scripts/data_collection.py b/scripts/data_collection.py index 2fe4cbf..4d1f4c0 100755 --- a/scripts/data_collection.py +++ b/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() diff --git a/src/BSCSolver.cpp b/src/BSCSolver.cpp index 2ce2b81..f3104dd 100644 --- a/src/BSCSolver.cpp +++ b/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("/jackal_bsc/bsc_vel", q_size); - alpha_pub = n.advertise("/jackal_bsc/alpha", q_size); + alpha_pub = n.advertise("/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);