diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/launch/full_system_sim.launch b/Software/real_time/ROS_RoboBuggy/src/robobuggy/launch/full_system_sim.launch
new file mode 100644
index 00000000..243163da
--- /dev/null
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/launch/full_system_sim.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/msg/Pose.msg b/Software/real_time/ROS_RoboBuggy/src/robobuggy/msg/Pose.msg
index 05ee46a0..cdad154a 100644
--- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/msg/Pose.msg
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/msg/Pose.msg
@@ -3,6 +3,5 @@ Header header
float64 latitude_deg
float64 longitude_deg
-# where 0 means north, pi/2 means east, pi means south, 3pi/2 means west
-# this value should ALWAYS be modded by 2pi
+# Cartesian coordinate system
float64 heading_rad
\ No newline at end of file
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/data_map_plot flowchart.graphml b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/data_map_plot flowchart.graphml
index b403310d..a33294b0 100755
--- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/data_map_plot flowchart.graphml
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/data_map_plot flowchart.graphml
@@ -1,6 +1,6 @@
-
+
@@ -350,6 +350,134 @@
+
+
+
+
+
+
+ Receive GPS Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Create new GPSFix message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Fill GPSFix message with lat and lon
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish GPS viz message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Receive ground truth Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Create new GPSFix message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Fill GPSFix message with lat, lon, and heading
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish GPS viz message
+
+
+
+
+
+
+
+
+
@@ -613,6 +741,98 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.graphml b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.graphml
new file mode 100644
index 00000000..1ad5ed0d
--- /dev/null
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.graphml
@@ -0,0 +1,772 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Node Start
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Node Initialization
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Determine Start Position
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Propagate Simulator Forwards
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Recalculate Motion Model
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish GPS Message?
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish Odometry Message?
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish IMU Message?
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish GPS Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish Odometry Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish IMU Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish GPS Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Get current location (x, y)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Add Gaussian noise
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish Odometry Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Get current velocity
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Add Gaussian noise
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish IMU Message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Get current orientation
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Add Gaussian noise
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Publish message
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Shutdown Signal Received
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ <exit>
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ yes
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ no
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ yes
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ yes
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ no
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ no
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.pdf b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.pdf
new file mode 100644
index 00000000..9dfc0d8a
Binary files /dev/null and b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/Doc_Flowchart_Source/sim/full_system_sim.pdf differ
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot flowchart.pdf b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot flowchart.pdf
index 29d8d89e..2813881c 100644
Binary files a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot flowchart.pdf and b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot flowchart.pdf differ
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot.py
index 520bdd1c..f6ca13e9 100755
--- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot.py
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/data_map_plot.py
@@ -13,7 +13,7 @@
def pose_callback(data):
- global viz_pub
+ global viz_pose_pub
global last_latitude
global last_longitude
global last_heading_deg
@@ -25,12 +25,17 @@ def pose_callback(data):
last_longitude = data.longitude_deg
last_heading_deg = -degrees_from_north + 90
- viz_msg_heading = GPSFix(latitude=last_latitude, longitude=last_longitude, track = last_heading_deg)
rospy.loginfo("got Pose msg: %f degrees lat, %f degrees long, %f bearing", last_latitude, last_longitude, last_heading_deg)
- viz_pub.publish(viz_msg_heading)
+ viz_msg_heading = GPSFix(latitude=last_latitude, longitude=last_longitude, track=last_heading_deg)
+ viz_pose_pub.publish(viz_msg_heading)
- pass
+
+def gps_callback(data):
+ global viz_gps_pub
+ rospy.loginfo("Got GPS message: %f lat, %f lon", data.Lat_deg, data.Long_deg)
+ viz_msg_gps = GPSFix(latitude=data.Lat_deg, longitude=data.Long_deg)
+ viz_gps_pub.publish(viz_msg_gps)
def command_callback(data):
global viz_command_pub
@@ -47,12 +52,23 @@ def command_callback(data):
pass
+def ground_truth_callback(data):
+ global viz_grndtruth_pub
+
+ degrees_from_north = -math.degrees(data.heading_rad) + 90
+
+ rospy.loginfo("Got Ground Truth message: (%f, %f, %f)", data.latitude_deg, data.longitude_deg, degrees_from_north)
+
+ viz_msg_groundtruth = GPSFix(latitude=data.latitude_deg, longitude=data.longitude_deg, track=degrees_from_north)
+ viz_grndtruth_pub.publish(viz_msg_groundtruth)
+ pass
+
def waypoints_publisher():
waypoints_pub = rospy.Publisher('WAYPOINTS_VIZ',GPSFix, queue_size=10)
# read from waypoints file, parse JSON, publish
- waypoints = open("../RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt", 'r')
+ waypoints = open("/mnt/c/Users/bhai/Documents/RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt", 'r')
line = waypoints.readline()
time.sleep(5)
while line:
@@ -65,25 +81,31 @@ def waypoints_publisher():
pass
def start_subscriber_spin():
- global viz_pub
+ global viz_gps_pub
+ global viz_pose_pub
global viz_command_pub
+ global viz_grndtruth_pub
global last_latitude
global last_longitude
global last_heading_deg
+ last_heading_deg = 0
- rospy.init_node("GPS_Plotter", anonymous=True)
+ rospy.init_node("Data Visualizer", anonymous=True)
- viz_pub = rospy.Publisher('GPS_VIZ', GPSFix, queue_size=10)
+ viz_gps_pub = rospy.Publisher("GPS_VIZ", GPSFix, queue_size=10)
+ viz_pose_pub = rospy.Publisher("POSE_VIZ", GPSFix, queue_size=10)
viz_command_pub = rospy.Publisher('STEERING_COMMAND_VIZ', GPSFix, queue_size = 10)
+ viz_grndtruth_pub = rospy.Publisher("SIM_GROUNDTRUTH_VIZ", GPSFix, queue_size=10)
rospy.Subscriber("Pose", Pose, pose_callback)
+ rospy.Subscriber("GPS", GPS, gps_callback)
rospy.Subscriber("Command", Command, command_callback)
+ rospy.Subscriber("SIM_Ground_Truth", Pose, ground_truth_callback)
waypoints_publisher()
rospy.spin()
- pass
if __name__ == "__main__":
start_subscriber_spin()
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/controller_simulator.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/sim/controller_simulator.py
similarity index 100%
rename from Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/controller_simulator.py
rename to Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/sim/controller_simulator.py
diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/sim/full_system_sim.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/sim/full_system_sim.py
new file mode 100644
index 00000000..107d8a94
--- /dev/null
+++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/sim/full_system_sim.py
@@ -0,0 +1,167 @@
+#!/usr/bin/env python
+
+# Full System Simulator
+# Keeps track of a ground-truth position for the robot, but reports a noisy version to the rest of the system
+
+# TODO Currently reports only GPS and encoder, include IMU as well
+import rospy
+import numpy as np
+import utm
+import math
+from robobuggy.msg import GPS
+from robobuggy.msg import Encoder
+from robobuggy.msg import Command
+from robobuggy.msg import Pose
+import json
+import time
+
+class Simulator:
+
+ def __init__(self, sigma_gps, sigma_odom, start_position):
+ self.sigma_gps = sigma_gps
+ self.sigma_odom = sigma_odom
+ self.steering_angle = 0
+ self.sim_update_hz = 10
+ self.dt = 1.0 / self.sim_update_hz
+ self.wheelbase = 1.13
+ self.gps_update_rate_hz = 2
+ self.odom_update_rate_hz = 5
+ self.imu_update_rate_hz = 5
+ self.ground_truth_update_rate_hz = 5
+ self.utm_letter = 'T'
+ self.utm_zone = 17
+ self.velocity = 3
+
+ x = [
+ start_position[0],
+ start_position[1],
+ self.velocity,
+ start_position[2],
+ 0
+ ]
+ self.x = np.array(x)
+ self.x = np.reshape(x, [5,1])
+
+ def apply_control_input(self, data):
+ self.steering_angle = data.steer_cmd_rad
+
+ def calculate_new_motion_model(self):
+ A = [
+ [1, 0, self.dt * np.cos(self.x[3]), 0, 0],
+ [0, 1, self.dt * np.sin(self.x[3]), 0, 0],
+ [0, 0, 1, 0, 0],
+ [0, 0, 0, 1, self.dt],
+ [0, 0, math.tan(self.steering_angle)/self.wheelbase, 0, 0]
+ ]
+ return np.matrix(A)
+
+ def step(self):
+ A = self.calculate_new_motion_model()
+ self.x = A * self.x
+ pass
+
+ def generate_gps_message(self):
+ (x, y) = (self.x[0], self.x[1])
+ x = np.random.normal(x, self.sigma_gps)
+ y = np.random.normal(y, self.sigma_gps)
+
+ (lat, lon) = utm.to_latlon(x, y, self.utm_zone, self.utm_letter)
+
+ noisy_msg = GPS()
+ noisy_msg.Lat_deg = lat
+ noisy_msg.Long_deg = lon
+ noisy_msg.easting = x
+ noisy_msg.northing = y
+
+ return noisy_msg
+
+ def generate_odometry_message(self):
+ approx_distance = self.velocity * self.dt * self.sim_update_hz / self.odom_update_rate_hz
+ approx_distance = np.random.normal(approx_distance, self.sigma_odom)
+
+ noisy_msg = Encoder()
+ noisy_msg.ticks = approx_distance * 1.0 / (0.61 / 7.0 * 0.3048 * 2)
+
+ return noisy_msg
+
+ def generate_ground_truth_message(self):
+ (x, y) = (self.x[0], self.x[1])
+ (lat, lon) = utm.to_latlon(x, y, self.utm_zone, self.utm_letter)
+
+ msg = Pose()
+ msg.latitude_deg = lat
+ msg.longitude_deg = lon
+ msg.heading_rad = self.x[3]
+
+ return msg
+
+ def generate_imu_message(self):
+ # TODO
+ pass
+
+def main():
+
+ # init
+ NODE_NAME = "Full_System_Simulator"
+ rospy.init_node(NODE_NAME)
+ gps_pub = rospy.Publisher('GPS', GPS, queue_size=10)
+ odom_pub = rospy.Publisher('Encoder', Encoder, queue_size=10)
+ ground_truth_pub = rospy.Publisher("SIM_Ground_Truth", Pose, queue_size=10)
+ # TODO
+ # imu_pub = rospy.Publisher('IMU', IMU, queue_size=10)
+
+ # 5m stddev
+ sigma_gps = 0.5
+
+ # 0.1m stddev
+ sigma_odom = 0.1
+
+ start_x = 0
+ start_y = 0
+ start_th = math.radians(250) # TODO calculate based on two waypoints
+ # waypoint_file = rospy.get_param("/{}/waypoint_file".format(NODE_NAME))
+ waypoint_file = "/mnt/c/Users/bhai/Documents/RoboBuggy/Software/real_time/ROS_RoboBuggy/src/robobuggy/config/waypoints.txt"
+ with open(waypoint_file) as f:
+ first_waypoint_str = f.readline()
+ first_waypoint_json = json.loads(first_waypoint_str)
+ first_waypoint = utm.from_latlon(first_waypoint_json['latitude'], first_waypoint_json['longitude'])
+ start_x = first_waypoint[0]
+ start_y = first_waypoint[1]
+
+ s = Simulator(sigma_gps, sigma_odom, (start_x, start_y, start_th))
+ command_sub = rospy.Subscriber('Command', Command, s.apply_control_input)
+
+ # spin infinitely, while stepping each appropriate tick
+ rate = rospy.Rate(s.sim_update_hz)
+ loop_counter = 0
+ while not rospy.is_shutdown():
+ # Take a step
+ s.step()
+
+ # If needed, generate and publish messages
+ if loop_counter % (s.sim_update_hz / s.gps_update_rate_hz) == 0:
+ gps_msg = s.generate_gps_message()
+ gps_pub.publish(gps_msg)
+
+ if loop_counter % (s.sim_update_hz / s.odom_update_rate_hz) == 0:
+ odom_msg = s.generate_odometry_message()
+ odom_pub.publish(odom_msg)
+
+ if loop_counter % (s.sim_update_hz / s.ground_truth_update_rate_hz) == 0:
+ gt_msg = s.generate_ground_truth_message()
+ ground_truth_pub.publish(gt_msg)
+
+ # TODO
+ # if loop_counter % s.imu_update_rate_hz:
+ # imu_msg = s.generate_imu_message()
+ # imu_pub.publish(imu_msg)
+
+ loop_counter += 1
+ rate.sleep()
+
+
+
+
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file