From 5b58ce52799e487dfd4bcf573a1dee865c89681b Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sat, 10 Apr 2021 18:56:37 -0500 Subject: [PATCH 1/5] Updated odometry to account for turning --- .../include/odometry/FollowerOdometry.h | 2 ++ src/v5_hal/firmware/include/util/Constants.h | 8 ++++++ .../src/odometry/FollowerOdometry.cpp | 25 ++++++++++++------- 3 files changed, 26 insertions(+), 9 deletions(-) diff --git a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h index 3534e1b7..bef45640 100644 --- a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h +++ b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h @@ -3,6 +3,8 @@ #include "odometry/Odometry.h" #include "util/Constants.h" +#include + class FollowerOdometry : public Odometry { private: diff --git a/src/v5_hal/firmware/include/util/Constants.h b/src/v5_hal/firmware/include/util/Constants.h index 4cd681c8..74b2de79 100644 --- a/src/v5_hal/firmware/include/util/Constants.h +++ b/src/v5_hal/firmware/include/util/Constants.h @@ -27,4 +27,12 @@ #define POSE_PURSUIT_LINEAR_THRESHOLD 5.0 #define POSE_PURSUIT_THETA_THRESHOLD 0.3 +#define ODOM_TRACK_WIDTH 0 + +#define ODOM_PERPENDICULAR_X -0.8 +#define ODOM_PERPENDICULAR_Y -5.478 + +#define ODOM_PARALLEL_X 1.75 +#define ODOM_PARALLEL_Y -3.924 + #endif diff --git a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp index b1afafe9..083fd164 100644 --- a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp +++ b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp @@ -17,20 +17,27 @@ FollowerOdometry::FollowerOdometry(EncoderConfig xEncoderConfig, EncoderConfig y */ void FollowerOdometry::Update(double x_encoder_raw_ticks, double y_encoder_raw_ticks, Rotation2Dd gyro_angle) { // Convert the current position in ticks to a position in distance units - double x_dist = x_encoder_raw_ticks * Odometry::m_encoder_1_ticks_to_dist; - double y_dist = y_encoder_raw_ticks * Odometry::m_encoder_2_ticks_to_dist; + double x_encoder_dist = x_encoder_raw_ticks * Odometry::m_encoder_1_ticks_to_dist; + double y_encoder_dist = y_encoder_raw_ticks * Odometry::m_encoder_2_ticks_to_dist; + + Vector2d x_encoder_location(ODOM_PERPENDICULAR_X, ODOM_PERPENDICULAR_Y); + Vector2d y_encoder_location(ODOM_PARALLEL_X, ODOM_PARALLEL_Y); // Reset the current position of the robot if (m_pose_reset) { - Odometry::m_last_encoder_1_dist = x_dist; - Odometry::m_last_encoder_2_dist = y_dist; + Odometry::m_last_encoder_1_dist = x_encoder_dist; + Odometry::m_last_encoder_2_dist = y_encoder_dist; Odometry::m_gyro_initial_angle = gyro_angle; Odometry::m_pose_reset = false; } - // Calculate the change in position since the last check - double x_delta = x_dist - Odometry::m_last_encoder_1_dist; - double y_delta = y_dist - Odometry::m_last_encoder_2_dist; + // Calculate the change in position of each encoder since the last check + double x_encoder_delta = x_encoder_dist - Odometry::m_last_encoder_1_dist; + double y_encoder_delta = y_encoder_dist - Odometry::m_last_encoder_2_dist; + Rotation2Dd phi = gyro_angle * m_robot_pose.angle.inverse(); // Find change in angle + + double x_delta = x_encoder_delta - (x_encoder_location.norm() * phi.angle() * sin(atan(x_encoder_location.y() / x_encoder_location.x()))); + double y_delta = y_encoder_delta - (y_encoder_location.norm() * phi.angle() * cos(atan(y_encoder_location.y() / y_encoder_location.x()))); // Convert the x and y deltas into a translation vector Vector2d robot_translation(x_delta, y_delta); @@ -46,6 +53,6 @@ void FollowerOdometry::Update(double x_encoder_raw_ticks, double y_encoder_raw_t Odometry::m_robot_pose.position += robot_translation; // Update the previous values of the encoders for the next iteration - Odometry::m_last_encoder_1_dist = x_dist; - Odometry::m_last_encoder_2_dist = y_dist; + Odometry::m_last_encoder_1_dist = x_encoder_dist; + Odometry::m_last_encoder_2_dist = y_encoder_dist; } \ No newline at end of file From 07755a760f40631e125a76b80a7a65e2a4776ab0 Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Sun, 11 Apr 2021 11:15:15 -0500 Subject: [PATCH 2/5] Fixed inertial node and configured robot after correctly switching encoders --- .vscode/settings.json | 3 +- .../nodes/sensor_nodes/InertialSensorNode.h | 4 +++ .../include/odometry/FollowerOdometry.h | 1 + src/v5_hal/firmware/include/util/Constants.h | 3 ++ src/v5_hal/firmware/src/main.cpp | 6 ++-- .../src/nodes/odometry_nodes/OdometryNode.cpp | 18 ++++------- .../nodes/sensor_nodes/InertialSensorNode.cpp | 32 ++++++++++++------- .../src/odometry/FollowerOdometry.cpp | 12 +++++-- 8 files changed, 49 insertions(+), 30 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 607193e9..0abeac57 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -83,7 +83,8 @@ "*.ipp": "cpp", "core": "cpp", "forward_list": "cpp", - "dense": "cpp" + "dense": "cpp", + "*.txx": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "Google" } \ No newline at end of file diff --git a/src/v5_hal/firmware/include/nodes/sensor_nodes/InertialSensorNode.h b/src/v5_hal/firmware/include/nodes/sensor_nodes/InertialSensorNode.h index d588f83a..543fe116 100644 --- a/src/v5_hal/firmware/include/nodes/sensor_nodes/InertialSensorNode.h +++ b/src/v5_hal/firmware/include/nodes/sensor_nodes/InertialSensorNode.h @@ -25,6 +25,8 @@ class InertialSensorNode : public Node { bool isAtAngle(Eigen::Rotation2Dd angle); + void reset(); + void teleopPeriodic(); void autonPeriodic(); @@ -43,4 +45,6 @@ class InertialSensorNode : public Node { ros::Subscriber* m_inertial_sensor_sub; void m_handleSensorMsg(const v5_hal::RollPitchYaw& msg); + + Eigen::Rotation2Dd m_getV5Yaw(); }; \ No newline at end of file diff --git a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h index bef45640..13c1454c 100644 --- a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h +++ b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h @@ -2,6 +2,7 @@ #include "odometry/Odometry.h" #include "util/Constants.h" +#include "util/Logger.h" #include diff --git a/src/v5_hal/firmware/include/util/Constants.h b/src/v5_hal/firmware/include/util/Constants.h index 74b2de79..e2e3d1a1 100644 --- a/src/v5_hal/firmware/include/util/Constants.h +++ b/src/v5_hal/firmware/include/util/Constants.h @@ -5,6 +5,9 @@ #define BALL_PRESENT_THRESHOLD 2700 // Threshold for Indexing Sensor to detect ball #define MAX_MOTOR_VOLTAGE 12000 // Max voltage value for a motor #define DELAY_TIME_MILLIS 5 // Standard delay time used in NodeManager + +// Offset to apply to get the gyro into our correct frame of reference +// Forward is M_PI_2, so we rotate the gyro angle to match #define GYRO_OFFSET (M_PI_2) #define MAX_WHEEL_SPEED 1.1526 // Wheel speed for Holonomic Drive in m/s diff --git a/src/v5_hal/firmware/src/main.cpp b/src/v5_hal/firmware/src/main.cpp index 605607b3..54858247 100644 --- a/src/v5_hal/firmware/src/main.cpp +++ b/src/v5_hal/firmware/src/main.cpp @@ -92,8 +92,8 @@ void initialize() { bottom_conveyor_sensor, top_conveyor_sensor); /* Define the odometry components */ - x_odom_encoder = new ADIEncoderNode(node_manager, 'C', 'D', "xOdomEncoder", false); - y_odom_encoder = new ADIEncoderNode(node_manager, 'A', 'B', "yOdomEncoder", true); + x_odom_encoder = new ADIEncoderNode(node_manager, 'A', 'B', "xOdomEncoder", false); + y_odom_encoder = new ADIEncoderNode(node_manager, 'C', 'D', "yOdomEncoder", false); inertial_sensor = new InertialSensorNode(node_manager, "inertialSensor", 15); // Port 15 @@ -104,7 +104,7 @@ void initialize() { connection_checker_node = new ConnectionCheckerNode(node_manager); /* Define autonomous components */ - auton_manager_node = new AutonManagerNode(node_manager, holonomic_drive_node, conveyor_node, intake_node, odom_node, inertial_sensor); + //auton_manager_node = new AutonManagerNode(node_manager, holonomic_drive_node, conveyor_node, intake_node, odom_node, inertial_sensor); // Call the node manager to initialize all of the nodes above node_manager->initialize(); diff --git a/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp b/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp index be3ed056..a8d5993f 100644 --- a/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp +++ b/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp @@ -32,7 +32,9 @@ Odometry* OdometryNode::m_getOdomClass(OdomConfig config) { } void OdometryNode::initialize() { - + // Calibrate the gyro, and reset the orientation with the correct gyro angle + m_inertial_sensor_node->reset(); + setCurrentPose(Pose(Vector2d(0, 0), m_inertial_sensor_node->getYaw())); } void OdometryNode::setCurrentPose(Pose pose) { @@ -44,20 +46,14 @@ Pose OdometryNode::getCurrentPose() { } void OdometryNode::teleopPeriodic() { - Rotation2Dd current_angle(m_inertial_sensor_node->getYaw()); - - m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), current_angle); - //m_odom->Update(m_motor_1->getPosition(), m_motor_2->getPosition(), current_angle); + m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), m_inertial_sensor_node->getYaw()); - // Logger::logInfo("Robot position: " + std::to_string(m_odom->GetPose().position.x()) + " " + - // std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle())); + Logger::logInfo("Robot position: " + std::to_string(m_odom->GetPose().position.x()) + " " + + std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle())); } void OdometryNode::autonPeriodic() { - Rotation2Dd current_angle(m_inertial_sensor_node->getYaw()); - - m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), current_angle); - //m_odom->Update(m_motor_1->getPosition(), m_motor_2->getPosition(), current_angle); + m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), m_inertial_sensor_node->getYaw()); } OdometryNode::~OdometryNode() { diff --git a/src/v5_hal/firmware/src/nodes/sensor_nodes/InertialSensorNode.cpp b/src/v5_hal/firmware/src/nodes/sensor_nodes/InertialSensorNode.cpp index 21ad2e13..21c4ab3f 100644 --- a/src/v5_hal/firmware/src/nodes/sensor_nodes/InertialSensorNode.cpp +++ b/src/v5_hal/firmware/src/nodes/sensor_nodes/InertialSensorNode.cpp @@ -2,7 +2,7 @@ InertialSensorNode::InertialSensorNode(NodeManager* node_manager, std::string handle_name, int sensor_port) : Node(node_manager, 20), - m_yaw(0), m_gyro_offset_angle(-M_PI_2) { + m_yaw(0), m_gyro_offset_angle(GYRO_OFFSET) { m_handle_name = handle_name.insert(0, "sensor/"); m_config = V5; @@ -10,7 +10,7 @@ InertialSensorNode::InertialSensorNode(NodeManager* node_manager, } InertialSensorNode::InertialSensorNode(NodeManager* node_manager, std::string handle_name, - std::string subscribe_handle) : Node(node_manager, 20), m_yaw(0), m_gyro_offset_angle(M_PI_2), + std::string subscribe_handle) : Node(node_manager, 20), m_yaw(0), m_gyro_offset_angle(GYRO_OFFSET), m_sub_inertial_sensor_name(subscribe_handle) { m_handle_name = handle_name.insert(0, "sensor/"); m_config = ROS; @@ -20,8 +20,15 @@ InertialSensorNode::InertialSensorNode(NodeManager* node_manager, std::string ha } void InertialSensorNode::m_handleSensorMsg(const v5_hal::RollPitchYaw& msg) { + // Convert value to radians Eigen::Rotation2Dd current_angle(msg.yaw * (M_PI/180)); - m_yaw = (current_angle.inverse()) * m_gyro_offset_angle; + // Rotate by the initial angle to get our current yaw + m_yaw = current_angle.inverse() * m_gyro_offset_angle; +} + +Eigen::Rotation2Dd InertialSensorNode::m_getV5Yaw() { + Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180)); + return current_angle * m_gyro_offset_angle; } void InertialSensorNode::initialize() { @@ -45,29 +52,30 @@ bool InertialSensorNode::isAtAngle(Eigen::Rotation2Dd angle) { return fabs((m_yaw * angle.inverse()).smallestAngle()) < turning_threshold; } +void InertialSensorNode::reset() { + m_inertial_sensor->reset(); + pros::delay(5000); + m_yaw = m_getV5Yaw(); +} + void InertialSensorNode::teleopPeriodic() { switch (m_config) { case V5: if (!(m_inertial_sensor->is_calibrating())) { - Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180)); - m_yaw = current_angle * m_gyro_offset_angle.inverse(); + // Convert sensor input to radians, and reverse orientation + m_yaw = m_getV5Yaw(); } } - - // std::string msg = m_handle_name + " value: " + std::to_string(getYaw().angle()); - // m_handle->logwarn(msg.c_str()); } void InertialSensorNode::autonPeriodic() { switch (m_config) { case V5: if (!m_inertial_sensor->is_calibrating()) { - Eigen::Rotation2Dd current_angle(m_inertial_sensor->get_yaw() * -(M_PI/180)); - m_yaw = current_angle * m_gyro_offset_angle.inverse(); + // Convert sensor input to radians, and reverse orientation + m_yaw = m_getV5Yaw(); } } - - Logger::logInfo("Robot angle: " + std::to_string(getYaw().angle())); } InertialSensorNode::~InertialSensorNode () { diff --git a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp index 083fd164..54379b79 100644 --- a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp +++ b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp @@ -39,15 +39,21 @@ void FollowerOdometry::Update(double x_encoder_raw_ticks, double y_encoder_raw_t double x_delta = x_encoder_delta - (x_encoder_location.norm() * phi.angle() * sin(atan(x_encoder_location.y() / x_encoder_location.x()))); double y_delta = y_encoder_delta - (y_encoder_location.norm() * phi.angle() * cos(atan(y_encoder_location.y() / y_encoder_location.x()))); + // Logger::logInfo("Encoder: " + std::to_string(x_encoder_delta) + + // " | Modified: " + std::to_string(x_delta) + + // " | Change in angle: " + std::to_string(phi.angle())); + // Convert the x and y deltas into a translation vector Vector2d robot_translation(x_delta, y_delta); // Update the current angle of the robot position - Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Rotation2Dd(m_gyro_offset); - //Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Rotation2Dd(GYRO_OFFSET); + // Find the difference of the current angle to the initial angle + // Rotate this difference by the M_PI_2 offset to put it back in the correct frame of reference + Odometry::m_robot_pose.angle = gyro_angle * Odometry::m_gyro_initial_angle.inverse() * Odometry::m_gyro_offset; // Rotate the translation vector by the current angle rotation matrix - robot_translation = Odometry::m_robot_pose.angle * robot_translation; + // We need to rotate this back, as our encoders are oriented with 0 being forward + robot_translation = (Odometry::m_robot_pose.angle * Odometry::m_gyro_offset.inverse()) * robot_translation; // Add the current translation onto the robot position vector Odometry::m_robot_pose.position += robot_translation; From 32ab6ad9e402e6baa5eef547e7bf65657a03cc6e Mon Sep 17 00:00:00 2001 From: NathanDuPont Date: Fri, 16 Apr 2021 23:58:31 -0500 Subject: [PATCH 3/5] Added logging to test rotation math --- .../include/odometry/FollowerOdometry.h | 5 ++++- src/v5_hal/firmware/include/util/Constants.h | 4 ++-- .../src/nodes/odometry_nodes/OdometryNode.cpp | 5 +++-- .../src/odometry/FollowerOdometry.cpp | 21 ++++++++++++++----- 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h index 13c1454c..d28546b8 100644 --- a/src/v5_hal/firmware/include/odometry/FollowerOdometry.h +++ b/src/v5_hal/firmware/include/odometry/FollowerOdometry.h @@ -8,7 +8,10 @@ class FollowerOdometry : public Odometry { private: - + float total_x = 0; + float total_y = 0; + float turn_x = 0; + float turn_y = 0; public: FollowerOdometry(EncoderConfig x_encoder_config, EncoderConfig y_encoder_config, Pose current_pose=Pose()); diff --git a/src/v5_hal/firmware/include/util/Constants.h b/src/v5_hal/firmware/include/util/Constants.h index e2e3d1a1..22e565b2 100644 --- a/src/v5_hal/firmware/include/util/Constants.h +++ b/src/v5_hal/firmware/include/util/Constants.h @@ -32,10 +32,10 @@ #define ODOM_TRACK_WIDTH 0 -#define ODOM_PERPENDICULAR_X -0.8 +#define ODOM_PERPENDICULAR_X 0.8 #define ODOM_PERPENDICULAR_Y -5.478 -#define ODOM_PARALLEL_X 1.75 +#define ODOM_PARALLEL_X -1.75 #define ODOM_PARALLEL_Y -3.924 #endif diff --git a/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp b/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp index a8d5993f..01a3fdd6 100644 --- a/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp +++ b/src/v5_hal/firmware/src/nodes/odometry_nodes/OdometryNode.cpp @@ -48,8 +48,9 @@ Pose OdometryNode::getCurrentPose() { void OdometryNode::teleopPeriodic() { m_odom->Update(m_odom_encoder_1->getValue(), m_odom_encoder_2->getValue(), m_inertial_sensor_node->getYaw()); - Logger::logInfo("Robot position: " + std::to_string(m_odom->GetPose().position.x()) + " " + - std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle())); + // Logger::logInfo("Robot position: " + std::to_string(m_odom->GetPose().position.x()) + " " + + // std::to_string(m_odom->GetPose().position.y()) + " | Robot angle: " + std::to_string(m_odom->GetPose().angle.angle())); + //Logger::logInfo("x Encoder: " + std::to_string(m_odom_encoder_1->getValue()) + " | y Encoder: " + std::to_string(m_odom_encoder_2->getValue())); } void OdometryNode::autonPeriodic() { diff --git a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp index 54379b79..d8175cf8 100644 --- a/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp +++ b/src/v5_hal/firmware/src/odometry/FollowerOdometry.cpp @@ -34,17 +34,28 @@ void FollowerOdometry::Update(double x_encoder_raw_ticks, double y_encoder_raw_t // Calculate the change in position of each encoder since the last check double x_encoder_delta = x_encoder_dist - Odometry::m_last_encoder_1_dist; double y_encoder_delta = y_encoder_dist - Odometry::m_last_encoder_2_dist; - Rotation2Dd phi = gyro_angle * m_robot_pose.angle.inverse(); // Find change in angle + Rotation2Dd angle_delta = gyro_angle * m_robot_pose.angle.inverse(); // Find change in angle - double x_delta = x_encoder_delta - (x_encoder_location.norm() * phi.angle() * sin(atan(x_encoder_location.y() / x_encoder_location.x()))); - double y_delta = y_encoder_delta - (y_encoder_location.norm() * phi.angle() * cos(atan(y_encoder_location.y() / y_encoder_location.x()))); + double x_delta = x_encoder_delta - (x_encoder_location.norm() * angle_delta.angle() * fabs(sin(atan(x_encoder_location.y() / x_encoder_location.x())))); + double y_delta = y_encoder_delta - (-1 * y_encoder_location.norm() * angle_delta.angle() * fabs(cos(atan(y_encoder_location.y() / y_encoder_location.x())))); + + total_x = total_x + x_encoder_delta; + total_y = total_y + y_encoder_delta; + + turn_x = turn_x + (x_encoder_location.norm() * angle_delta.angle() * fabs(sin(atan(x_encoder_location.y() / x_encoder_location.x())))); + turn_y = turn_y + (y_encoder_location.norm() * angle_delta.angle() * fabs(cos(atan(y_encoder_location.y() / y_encoder_location.x())))); // Logger::logInfo("Encoder: " + std::to_string(x_encoder_delta) + // " | Modified: " + std::to_string(x_delta) + - // " | Change in angle: " + std::to_string(phi.angle())); + // " | Change in angle: " + std::to_string(angle_delta.angle())); + + Logger::logInfo("Total X: " + std::to_string(total_x) + + " | Total Y: " + std::to_string(total_y) + + " | Turn X: " + std::to_string(turn_x) + + " | Turn Y: " + std::to_string(turn_y)); // Convert the x and y deltas into a translation vector - Vector2d robot_translation(x_delta, y_delta); + Vector2d robot_translation(x_encoder_delta, y_encoder_delta); // Update the current angle of the robot position // Find the difference of the current angle to the initial angle From c12eb0d31c1a59fadf3c7db2e9a3e56312fee4c7 Mon Sep 17 00:00:00 2001 From: Dylan Powers Date: Sun, 6 Jun 2021 18:26:05 -0500 Subject: [PATCH 4/5] updated .gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 06874f45..8d57920e 100644 --- a/.gitignore +++ b/.gitignore @@ -20,4 +20,5 @@ temp.errors *.ini .d/ .vscode/settings.json +.vscode/c_cpp_properties.json .history/ From 8cab9c7dfaa213be84c057674292a764a90bca9d Mon Sep 17 00:00:00 2001 From: Dylan Powers Date: Sun, 6 Jun 2021 18:59:20 -0500 Subject: [PATCH 5/5] refactored Logger to publish to its own topic 'logger' --- src/v5_hal/firmware/include/nodes/NodeManager.h | 1 + src/v5_hal/firmware/include/util/Logger.h | 6 ++++++ src/v5_hal/firmware/src/nodes/NodeManager.cpp | 1 + src/v5_hal/firmware/src/util/Logger.cpp | 9 +++++++-- 4 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/v5_hal/firmware/include/nodes/NodeManager.h b/src/v5_hal/firmware/include/nodes/NodeManager.h index 708db3ad..789d0340 100644 --- a/src/v5_hal/firmware/include/nodes/NodeManager.h +++ b/src/v5_hal/firmware/include/nodes/NodeManager.h @@ -5,6 +5,7 @@ #include "api.h" #include "ros_lib/ros.h" #include "util/Constants.h" +#include "util/Logger.h" class Node; diff --git a/src/v5_hal/firmware/include/util/Logger.h b/src/v5_hal/firmware/include/util/Logger.h index 44cf8a59..8162936c 100644 --- a/src/v5_hal/firmware/include/util/Logger.h +++ b/src/v5_hal/firmware/include/util/Logger.h @@ -20,9 +20,15 @@ class Logger { WARNING, ERROR }; + + std::string m_message; + std::string m_handle_name = "logger"; + + static void initialize(); static void logInfo(string message); static void setConsoleLoggingLevel(LoggingLevel level); static void giveNodeManager(NodeManager * node_manager); + private: static LoggingLevel m_console_logging_level; static NodeManager * m_node_manager; diff --git a/src/v5_hal/firmware/src/nodes/NodeManager.cpp b/src/v5_hal/firmware/src/nodes/NodeManager.cpp index 0bca139e..3d37b61a 100644 --- a/src/v5_hal/firmware/src/nodes/NodeManager.cpp +++ b/src/v5_hal/firmware/src/nodes/NodeManager.cpp @@ -19,6 +19,7 @@ void NodeManager::initialize() { for (auto node_structure : m_node_structures) { node_structure.node->initialize(); } + Logger::initialize(); } void NodeManager::reset() { diff --git a/src/v5_hal/firmware/src/util/Logger.cpp b/src/v5_hal/firmware/src/util/Logger.cpp index e1eb0539..6400a622 100644 --- a/src/v5_hal/firmware/src/util/Logger.cpp +++ b/src/v5_hal/firmware/src/util/Logger.cpp @@ -3,14 +3,19 @@ Logger::LoggingLevel Logger::m_console_logging_level; NodeManager* Logger::m_node_manager = nullptr; +void Logger::initialize() { + m_publisher = new ros::Publisher(m_handle_name.c_str(), &m_message); + Node::m_handle->advertise(*m_publisher); +} + void Logger::giveNodeManager(NodeManager* node_manager) { m_node_manager = node_manager; } void Logger::logInfo(string message) { if(m_node_manager != nullptr) { - string msg = message; - m_node_manager->m_handle->logwarn(msg.c_str()); + m_message = message; + m_publisher->publish(&m_message); } }