big set of changes to make sure the whole stack works

Software/real_time/ROS_RoboBuggy/src/robobuggy/CMakeLists.txt

@@ -144,15 +144,15 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/robobuggy_node.cpp)
-add_executable(${PROJECT_NAME}_Controller_Runner include/json/jsoncpp.cpp src/controller/Controller_Runner.cpp src/controller/Controller.cpp)
-add_executable(${PROJECT_NAME}_IMU_Broadcaster_Runner src/imu/IMU_Broadcaster_Runner.cpp src/imu/IMU_Broadcaster.cpp)
-add_executable(${PROJECT_NAME}_Transistor_LL_Broadcaster_Runner src/low_level/Transistor_LL_Broadcaster_Runner.cpp src/low_level/Transistor_LL_Broadcaster.cpp)
-add_executable(${PROJECT_NAME}_Transistor_GPS_Broadcaster_Runner src/gps/Transistor_GPS_Broadcaster_Runner.cpp src/gps/Transistor_GPS_Broadcaster.cpp)
-add_executable(${PROJECT_NAME}_Transistor_Localizer_Runner src/localizer/Localizer_Runner.cpp src/localizer/Localizer.cpp)
-
-add_dependencies(${PROJECT_NAME}_Controller_Runner robobuggy_generate_messages_cpp)
-add_dependencies(${PROJECT_NAME}_IMU_Broadcaster_Runner robobuggy_generate_messages_cpp)
-add_dependencies(${PROJECT_NAME}_Transistor_LL_Broadcaster_Runner robobuggy_generate_messages_cpp)
+add_executable(${PROJECT_NAME}_Transistor_Controller_Runner include/json/jsoncpp.cpp src/transistor/controller/Controller_Runner.cpp src/transistor/controller/Controller.cpp)
+add_executable(${PROJECT_NAME}_Transistor_IMU_Broadcaster_Runner src/transistor/imu/IMU_Broadcaster_Runner.cpp src/transistor/imu/IMU_Broadcaster.cpp)
+add_executable(${PROJECT_NAME}_Transistor_LowLevel_Broadcaster_Runner src/transistor/low_level/LowLevel_Broadcaster_Runner.cpp src/transistor/low_level/LowLevel_Broadcaster.cpp)
+add_executable(${PROJECT_NAME}_Transistor_GPS_Broadcaster_Runner src/transistor/gps/GPS_Broadcaster_Runner.cpp src/transistor/gps/GPS_Broadcaster.cpp)
+add_executable(${PROJECT_NAME}_Transistor_Localizer_Runner src/transistor/localizer/Localizer_Runner.cpp src/transistor/localizer/Localizer.cpp)
+
+add_dependencies(${PROJECT_NAME}_Transistor_Controller_Runner robobuggy_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_Transistor_IMU_Broadcaster_Runner robobuggy_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME}_Transistor_LowLevel_Broadcaster_Runner robobuggy_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME}_Transistor_GPS_Broadcaster_Runner robobuggy_generate_messages_cpp)
 add_dependencies(${PROJECT_NAME}_Transistor_Localizer_Runner robobuggy_generate_messages_cpp)
 

Software/real_time/ROS_RoboBuggy/src/robobuggy/include/transistor/controller/Controller.h

@@ -7,7 +7,7 @@
 
 #include "ros/ros.h"
 #include <robobuggy/Pose.h>
-#include <robobuggy/Steering.h>
+#include <robobuggy/Command.h>
 #include <robobuggy/GPS.h>
 #include <string>
 #include <geodesy/utm.h>

Software/real_time/ROS_RoboBuggy/src/robobuggy/msg/Command.msg

@@ -1,4 +1,4 @@
 Header header
 
 bool brake_cmd
-int32 steer_cmd
+float32 steer_cmd_rad

Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/controller_simulator.py

@@ -53,12 +53,12 @@ def updateModel(self,x):
     def command_callback(self, data):
 
         #steer_cmd should be radians
-        rospy.loginfo("got Steering Command msg: %f degrees", data.steer_cmd)
+        rospy.loginfo("got Steering Command msg: %f degrees", math.degrees(data.steer_cmd_rad))
 
 
         #data.steer_cmd is the information we get
         #this method will have to modify the model
-        self.steering = data.steer_cmd
+        self.steering = data.steer_cmd_rad
         self.model = self.updateModel(self.x)
 
         pass
@@ -95,7 +95,6 @@ def start_simulator_spin():
 
         simulator_pub.publish(simulated_pose)
         c_tester.updateModel(c_tester.x)
-        print(c_tester.x[2][0])
         #rospy.spin()
 
         c_tester.rate.sleep()

Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot.py

@@ -39,9 +39,9 @@ def command_callback(data):
     global last_heading_deg
 
     #steer_cmd should be degrees
-    rospy.loginfo("got Steering Command msg: %f degrees", data.steer_cmd)
+    rospy.loginfo("got Steering Command msg: %f degrees", math.degrees(data.steer_cmd_rad))
 
-    viz_msg_steering = GPSFix(latitude=last_latitude, longitude=last_longitude, track = last_heading_deg + data.steer_cmd)
+    viz_msg_steering = GPSFix(latitude=last_latitude, longitude=last_longitude, track = last_heading_deg - math.degrees(data.steer_cmd_rad))
 
     viz_command_pub.publish(viz_msg_steering)
 
@@ -54,6 +54,7 @@ def waypoints_publisher():
     # read from waypoints file, parse JSON, publish
     waypoints = open("../RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt", 'r')
     line = waypoints.readline()
+    time.sleep(5)
     while line:
     	data = json.loads(line)
         viz_msg_waypoint = GPSFix(latitude=data['latitude'],longitude=data['longitude'])

Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/controller/Controller.cpp

@@ -11,23 +11,31 @@ void Controller::Pose_Callback(const robobuggy::Pose::ConstPtr& msg)
 Controller::Controller(std::vector<robobuggy::GPS>& initial_waypoint_list)
 {
     waypoint_list = std::vector<robobuggy::GPS>(initial_waypoint_list);
-    steering_pub = nh.advertise<robobuggy::Steering>("Steering_Command", 1000);
+    steering_pub = nh.advertise<robobuggy::Command>("Command", 1000);
     pose_sub = nh.subscribe<robobuggy::Pose>("Pose", 1000, &Controller::Pose_Callback, this);
+    last_closest_index = 0;
 }
 
 void Controller::update_steering_estimate()
 {
-    robobuggy::Steering steering_msg;
-    steering_msg.steer_feedback = 0;
-    steering_msg.steer_angle = pure_pursuit_controller();
+    if (current_pose_estimate.latitude_deg == 0 && current_pose_estimate.longitude_deg == 0)
+    {
+        // haven't initialized yet
+        ROS_INFO("No pose update yet!\n");
+        return;
+    }
+
+    robobuggy::Command steering_msg;
+    double steering_rad = pure_pursuit_controller();
+    steering_msg.steer_cmd_rad = steering_rad;
 
     steering_pub.publish(steering_msg);
 }
 
 double Controller::get_distance_from_pose(robobuggy::Pose pose_msg)
 {
-    double dx = (current_pose_estimate.longitude_deg - pose_msg.longitude_deg) * 84723.58765;
-    double dy = (current_pose_estimate.latitude_deg - pose_msg.latitude_deg) * 111319.9;
+    double dx = (current_pose_estimate.longitude_deg - pose_msg.longitude_deg) * 111319.9;
+    double dy = (current_pose_estimate.latitude_deg - pose_msg.latitude_deg) * 84723.58765;
 
     return std::sqrt(dx * dx + dy * dy);
 }
@@ -57,6 +65,7 @@ int Controller::get_closest_waypoint_index()
 double Controller::pure_pursuit_controller()
 {
     int closest_index = get_closest_waypoint_index();
+    ROS_INFO("closest waypoint = %d\n", closest_index);
     double k = 2.5;
     double velocity = 3; // TODO bake velocity into pose messages
     double lookahead_lower_bound = 5.0;
@@ -79,18 +88,20 @@ double Controller::pure_pursuit_controller()
         gpsPoseMessage.latitude_deg = waypoint_list.at(lookahead_index).Lat_deg;
         gpsPoseMessage.longitude_deg = waypoint_list.at(lookahead_index).Long_deg;
 
-        if (get_distance_from_pose(gpsPoseMessage) < lookahead)
+        if (get_distance_from_pose(gpsPoseMessage) > lookahead)
         {
             break;
         }
     }
 
     if (lookahead_index >= waypoint_list.size()) {
         // run out of waypoints nearby, go straight
+        ROS_INFO("uh oh, out of waypoints!\n");
         return 0.0;
     }
 
     robobuggy::GPS target_waypoint = waypoint_list.at(lookahead_index);
+    ROS_INFO("target waypoint = (%f, %f)\n", target_waypoint.Lat_deg, target_waypoint.Long_deg);
 
     geographic_msgs::GeoPoint gps_point;
     gps_point.latitude = target_waypoint.Lat_deg;
@@ -106,6 +117,8 @@ double Controller::pure_pursuit_controller()
     double dy = utm_waypoint.northing - utm_pose.northing;
     double theta = atan2(dy, dx) - current_pose_estimate.heading_rad;
 
+    ROS_INFO("dx = %f, dy = %f, waypoint heading = %f, current heading = %f, dtheta = %f\n", dx, dy, 180/M_PI*atan2(dy, dx), 180/M_PI*current_pose_estimate.heading_rad, 180/M_PI*normalize_angle_rad(theta));
+
     double commanded_angle = normalize_angle_rad(atan2(2 * 1.13 * sin(theta), lookahead));
     return commanded_angle;
 }

Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/controller/Controller_Runner.cpp

@@ -2,7 +2,7 @@
 // Created by bhai on 9/15/17.
 //
 
-#include "controller/Controller.h"
+#include "transistor/controller/Controller.h"
 #include <json/json.h>
 #include <fstream>
 

Software/real_time/ROS_RoboBuggy/src/robobuggy/src/transistor/low_level/LowLevel_Broadcaster.cpp

@@ -96,28 +96,31 @@ void LowLevel_Broadcaster::parse_serial_msg(std::string serial_msg)
 void LowLevel_Broadcaster::send_command(const robobuggy::Command::ConstPtr& msg) {
     // Construct and send the steering command
     uint8_t serial_msg[6];
-    int data = msg->steer_cmd;
+    double data = msg->steer_cmd_rad;
 
-    ROS_INFO("Steering command: %d\n", data);
+    // convert into hundredths of degrees
+    int steer_angle = data * 180 / M_PI * 100;
+
+    ROS_INFO("Steering command: %d\n", steer_angle);
     serial_msg[0] = RBSM_MID_MEGA_STEER_COMMAND;
-    serial_msg[1] = (data >> 24) & 0xFF;
-    serial_msg[2] = (data >> 16) & 0xFF;
-    serial_msg[3] = (data >> 8) & 0xFF;
-    serial_msg[4] = data & 0xFF;
+    serial_msg[1] = (steer_angle >> 24) & 0xFF;
+    serial_msg[2] = (steer_angle >> 16) & 0xFF;
+    serial_msg[3] = (steer_angle >> 8) & 0xFF;
+    serial_msg[4] = steer_angle & 0xFF;
     serial_msg[5] = RBSM_FOOTER;
 
     if (rb_serial.isOpen()) {
         rb_serial.write(serial_msg, 6);
     }
 
     // Construct and send the brake command
-    data = msg->brake_cmd;
+    int brake_data = msg->brake_cmd;
 
     serial_msg[0] = RBSM_MID_MEGA_AUTON_BRAKE_COMMAND;
-    serial_msg[1] = (data >> 24) & 0xFF;
-    serial_msg[2] = (data >> 16) & 0xFF;
-    serial_msg[3] = (data >> 8) & 0xFF;
-    serial_msg[4] = data & 0xFF;
+    serial_msg[1] = (brake_data >> 24) & 0xFF;
+    serial_msg[2] = (brake_data >> 16) & 0xFF;
+    serial_msg[3] = (brake_data >> 8) & 0xFF;
+    serial_msg[4] = brake_data & 0xFF;
     serial_msg[5] = RBSM_FOOTER;
 
     if (rb_serial.isOpen()) {