diff --git a/scripts/run-tests.sh b/scripts/run-tests.sh old mode 100755 new mode 100644 diff --git a/scripts/test.sh b/scripts/test.sh index ba46028f0..68f93baff 100755 --- a/scripts/test.sh +++ b/scripts/test.sh @@ -27,6 +27,12 @@ if [ -d $NET_DIR ]; then popd fi +# Set the environment variables +export ROS_LOG_DIR="/src/network_systems/scripts/can_transceiver.log" +export RCUTILS_COLORIZED_OUTPUT="1" + +echo "ROS_LOG_DIR is set to: $ROS_LOG_DIR" + colcon test --packages-ignore virtual_iridium --merge-install --event-handlers console_cohesion+ colcon test-result exit 0 diff --git a/src/network_systems/launch/main_launch.py b/src/network_systems/launch/main_launch.py index 9241dc518..742ca4696 100644 --- a/src/network_systems/launch/main_launch.py +++ b/src/network_systems/launch/main_launch.py @@ -8,7 +8,7 @@ from launch_ros.actions import Node from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, SetEnvironmentVariable from launch.launch_context import LaunchContext from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitutions import LaunchConfiguration @@ -81,6 +81,11 @@ def setup_launch(context: LaunchContext) -> List[Node]: Returns: List[Nodes]: Nodes to launch. """ + mode = LaunchConfiguration("mode").perform(context) + if mode == "development": + SetEnvironmentVariable( + name="ROS_LOG_DIR", value="/workspaces/sailbot_workspace/log" + ).visit(context) launch_description_entities = list() launch_description_entities.append(get_cached_fib_description(context)) launch_description_entities.append(get_mock_ais_description(context)) @@ -221,12 +226,12 @@ def get_remote_transceiver_description(context: LaunchContext) -> Node: def get_local_transceiver_description(context: LaunchContext) -> Node: """Gets the launch description for the local_transceiver_node. - Args: - context (LaunchContext): The current launch context. + Args: + context (LaunchContext): The current launch context. - Returns: - Node: The node object that launches the local_transceiver_node. - """ + Returns: + Node: The node object that launches the local_transceiver_node. + """ node_name = "local_transceiver_node" ros_parameters = [ global_launch_config, diff --git a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h index 4d7c9733d..e6b3fe84e 100644 --- a/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h +++ b/src/network_systems/projects/can_transceiver/inc/can_frame_parser.h @@ -135,6 +135,12 @@ class BaseFrame * @return A string that can be printed or logged for debugging */ virtual std::string debugStr() const; + + /** + * @brief A string representation of the dataframe + * + */ + virtual std::string toString() const; }; /** @@ -186,6 +192,12 @@ class Battery final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the Battery object + * + */ + std::string toString() const override; + private: /** * @brief Private helper constructor for Battery objects @@ -252,6 +264,12 @@ class MainTrimTab final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the MainTrimTab object + * + */ + std::string toString() const override; + private: /** * @brief Private helper constructor for MainTrimTab objects @@ -317,6 +335,12 @@ class WindSensor final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the WindSensor object + * + */ + std::string toString() const override; + /** * @brief Factory method to convert the index of a wind sensor in the custom_interfaces ROS representation * into a CanId if valid. @@ -397,6 +421,12 @@ class GPS final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the GPS object + * + */ + std::string toString() const override; + private: /** * @brief Private helper constructor for GPS objects @@ -476,6 +506,12 @@ class AISShips final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the AISShips object + * + */ + std::string toString() const override; + /** * @brief Returns the number of ships * @@ -639,6 +675,12 @@ class DesiredHeading final : public BaseFrame */ std::string debugStr() const override; + /** + * @brief A string representation of the DesiredHeading object + * + */ + std::string toString() const override; + private: /** * @brief Private helper constructor for DesiredHeading objects diff --git a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp index 10ee448cb..6a82fd8f5 100644 --- a/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp @@ -66,6 +66,8 @@ BaseFrame::BaseFrame(std::span valid_ids, CanId id, uint8_t can_byt std::string BaseFrame::debugStr() const { return CanDebugStr(id_); } +std::string BaseFrame::toString() const { return CanIdToStr(id_); } + CanFrame BaseFrame::toLinuxCan() const { return CanFrame{.can_id = static_cast(id_), .len = can_byte_dlen_}; } // BaseFrame protected END @@ -123,6 +125,13 @@ std::string Battery::debugStr() const return ss.str(); } +std::string Battery::toString() const +{ + std::stringstream ss; + ss << "[BATTERY] Voltage: " << volt_; + return ss.str(); +} + // Battery public END // Battery private START @@ -190,6 +199,13 @@ std::string MainTrimTab::debugStr() const return ss.str(); } +std::string MainTrimTab::toString() const +{ + std::stringstream ss; + ss << "[MAIN TRIM TAB] Angle: " << angle_; + return ss.str(); +} + // MainTrimTab public END // MainTrimTab private START @@ -263,6 +279,13 @@ std::string WindSensor::debugStr() const return ss.str(); } +std::string WindSensor::toString() const +{ + std::stringstream ss; + ss << "[WIND SENSOR] Speed: " << wind_speed_ << " Angle: " << wind_angle_; + return ss.str(); +} + std::optional WindSensor::rosIdxToCanId(size_t wind_idx) { if (wind_idx < WIND_SENSOR_IDS.size()) { @@ -381,6 +404,13 @@ std::string GPS::debugStr() const return ss.str(); } +std::string GPS::toString() const +{ + std::stringstream ss; + ss << "[GPS] Latitude: " << lat_ << " Longitude: " << lon_ << " Speed: " << speed_; //NOTE HEADING IS NOT USED + return ss.str(); +} + //GPS public END //GPS private START @@ -550,6 +580,13 @@ std::string AISShips::debugStr() const return ss.str(); } + +std::string AISShips::toString() const +{ + std::stringstream ss; + ss << "[AIS SHIP] ID: " << ship_id_ << " Latitude: " << lat_ << " Longitude: " << lon_; + return ss.str(); +} //AISShips public END //AISShips private START @@ -698,6 +735,13 @@ std::string DesiredHeading::debugStr() const return ss.str(); } +std::string DesiredHeading::toString() const +{ + std::stringstream ss; + ss << "[DESIRED HEADING] Heading: " << heading_; + return ss.str(); +} + // DesiredHeading public END // DesiredHeading private START diff --git a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp index 84fbfd96b..18080298d 100644 --- a/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp +++ b/src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp @@ -103,7 +103,7 @@ class CanTransceiverIntf : public NetNode this->create_publisher(ros_topics::BOAT_SIM_INPUT, QUEUE_SIZE); timer_ = this->create_wall_timer(TIMER_INTERVAL, [this]() { - mockBatteriesCb(); + //mockBatteriesCb(); publishBoatSimInput(boat_sim_input_msg_); // Add any other necessary looping callbacks }); @@ -172,6 +172,7 @@ class CanTransceiverIntf : public NetNode ais_ships_holder_.clear(); ais_ships_num_ = 0; // reset the number of ships } + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), ais_ship.toString().c_str()); } /** @@ -195,6 +196,17 @@ class CanTransceiverIntf : public NetNode } CAN_FP::PwrMode power_mode(set_pwr_mode, CAN_FP::CanId::PWR_MODE); can_trns_->send(power_mode.toLinuxCan()); + + // Get the current time as a time_point + auto now = std::chrono::system_clock::now(); + + // Convert it to a time_t object for extracting hours and minutes + std::time_t currentTime = std::chrono::system_clock::to_time_t(now); + + std::stringstream ss; + ss << currentTime; + + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), bat.toString().c_str()); } /** @@ -209,6 +221,7 @@ class CanTransceiverIntf : public NetNode msg::GPS gps_ = gps.toRosMsg(); gps_pub_->publish(gps_); + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), gps.toString().c_str()); } /** @@ -229,8 +242,8 @@ class CanTransceiverIntf : public NetNode msg::WindSensor & wind_sensor_msg = wind_sensors_.wind_sensors[idx]; wind_sensor_msg = wind_sensor.toRosMsg(); wind_sensors_pub_->publish(wind_sensors_); - publishFilteredWindSensor(); + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), wind_sensor.toString().c_str()); } /** @@ -260,6 +273,9 @@ class CanTransceiverIntf : public NetNode filtered_wind_sensor_.set__direction(static_cast(average_direction)); filtered_wind_sensor_pub_->publish(filtered_wind_sensor_); + std::stringstream ss; + ss << "[WIND SENSOR] Speed: " << filtered_speed.speed << " Angle: " << average_direction; + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), ss.str().c_str()); } /** @@ -286,6 +302,9 @@ class CanTransceiverIntf : public NetNode generic_sensor_msg.set__id(generic_frame.can_id); generic_sensors_pub_->publish(generic_sensors_); + std::stringstream ss; + ss << "[GENERIC SENSOR] CanID: " << generic_frame.can_id << " Data: " << generic_data; + RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), ss.str().c_str()); } // SIMULATION CALLBACKS // @@ -309,8 +328,10 @@ class CanTransceiverIntf : public NetNode { sail_cmd_ = sail_cmd_input; boat_sim_input_msg_.set__sail_cmd(sail_cmd_); - - can_trns_->send(CAN_FP::MainTrimTab(sail_cmd_input, CanId::MAIN_TR_TAB).toLinuxCan()); + auto main_trim_tab_frame = CAN_FP::MainTrimTab(sail_cmd_input, CanId::MAIN_TR_TAB); + can_trns_->send(main_trim_tab_frame.toLinuxCan()); + RCLCPP_INFO( + this->get_logger(), "%s %s", getCurrentTimeString().c_str(), main_trim_tab_frame.toString().c_str()); } /** @@ -331,7 +352,10 @@ class CanTransceiverIntf : public NetNode void subDesiredHeadingCb(msg::DesiredHeading desired_heading) { boat_sim_input_msg_.set__heading(desired_heading); - can_trns_->send(CAN_FP::DesiredHeading(desired_heading, CanId::MAIN_TR_TAB).toLinuxCan()); + auto desired_heading_frame = CAN_FP::DesiredHeading(desired_heading, CanId::MAIN_TR_TAB); + can_trns_->send(desired_heading_frame.toLinuxCan()); + RCLCPP_INFO( + this->get_logger(), "%s %s", getCurrentTimeString().c_str(), desired_heading_frame.toString().c_str()); } /** @@ -361,6 +385,18 @@ class CanTransceiverIntf : public NetNode can_trns_->send(CAN_FP::Battery(bat, CanId::BMS_DATA_FRAME).toLinuxCan()); } + static std::string getCurrentTimeString() + { + auto now = rclcpp::Clock().now(); + std::time_t time_now = static_cast(now.seconds()); + std::tm time_info; + localtime_r(&time_now, &time_info); + + std::ostringstream ss; + ss << '[' << std::put_time(&time_info, "%Y-%m-%d %H:%M:%S") << ']'; + + return ss.str(); + } }; int main(int argc, char * argv[]) diff --git a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp index 17414b152..aa839606a 100644 --- a/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp +++ b/src/network_systems/projects/can_transceiver/test/test_can_transceiver.cpp @@ -1157,9 +1157,8 @@ TEST_F(TestCanTransceiver, TestNewGPSValid) */ TEST_F(TestCanTransceiver, TestNewDataWindValid) { - volatile bool is_cb_called = false; - - std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { + volatile bool is_cb_called = false; + std::function test_cb = [&is_cb_called](CAN_FP::CanFrame /*unused*/) { is_cb_called = true; }; canbus_t_->registerCanCbs({{