Bug fixes: fixed color map and properly clear diags data on disconnect
Bug fixes: fixed color map and properly clear diags data on disconnect

BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS,BCLab-UNM/SwarmBaseCode-ROS