diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index dc343b9..d034fc7 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -17,7 +17,7 @@ jobs: strategy: matrix: env: - - { ROS_DISTRO: dashing, ROS_REPO: ros } + - { ROS_DISTRO: foxy, ROS_REPO: ros } runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 diff --git a/raspimouse/CMakeLists.txt b/raspimouse/CMakeLists.txt index e965eed..3c1b2fe 100644 --- a/raspimouse/CMakeLists.txt +++ b/raspimouse/CMakeLists.txt @@ -53,7 +53,7 @@ ament_target_dependencies(raspimouse tf2_ros raspimouse_msgs) -ament_export_interfaces(export_raspimouse_component HAS_LIBRARY_TARGET) +ament_export_targets(export_raspimouse_component HAS_LIBRARY_TARGET) ament_export_dependencies(rosidl_default_runtime) ament_export_dependencies(rclcpp) diff --git a/raspimouse/src/raspimouse_component.cpp b/raspimouse/src/raspimouse_component.cpp index 9902bfc..4000214 100644 --- a/raspimouse/src/raspimouse_component.cpp +++ b/raspimouse/src/raspimouse_component.cpp @@ -22,7 +22,7 @@ #include #include #include "rclcpp/rclcpp.hpp" -#include "rosidl_generator_cpp/message_initialization.hpp" +#include "rosidl_runtime_cpp/message_initialization.hpp" #include "lifecycle_msgs/msg/transition.hpp" using namespace std::chrono_literals; @@ -46,8 +46,8 @@ namespace raspimouse Raspimouse::Raspimouse(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("raspimouse", options), ros_clock_(RCL_ROS_TIME), - odom_(rosidl_generator_cpp::MessageInitialization::ZERO), - odom_transform_(rosidl_generator_cpp::MessageInitialization::ZERO), + odom_(rosidl_runtime_cpp::MessageInitialization::ZERO), + odom_transform_(rosidl_runtime_cpp::MessageInitialization::ZERO), last_odom_time_(0), linear_velocity_(0), angular_velocity_(0), @@ -118,12 +118,14 @@ CallbackReturn Raspimouse::on_configure(const rclcpp_lifecycle::State &) light_sensors_pub_ = this->create_publisher( "light_sensors", 10); // Timer for publishing switch information - switches_timer_ = create_wall_timer(100ms, std::bind( - &Raspimouse::publish_switches, this)); + switches_timer_ = create_wall_timer( + 100ms, std::bind( + &Raspimouse::publish_switches, this)); switches_timer_->cancel(); // Timer for publishing light sensor information - light_sensors_timer_ = create_wall_timer(100ms, std::bind( - &Raspimouse::publish_light_sensors, this)); + light_sensors_timer_ = create_wall_timer( + 100ms, std::bind( + &Raspimouse::publish_light_sensors, this)); light_sensors_timer_->cancel(); // Subscriber for LED commands leds_sub_ = create_subscription( @@ -493,7 +495,8 @@ void Raspimouse::calculate_odometry_from_pulse_counts(double & x, double & y, do int pulse_count_left, pulse_count_right; left_counter >> pulse_count_left; right_counter >> pulse_count_right; - RCLCPP_DEBUG(get_logger(), "Old: %d, %d\tNew: %d, %d", last_pulse_count_left_, + RCLCPP_DEBUG( + get_logger(), "Old: %d, %d\tNew: %d, %d", last_pulse_count_left_, last_pulse_count_right_, pulse_count_left, pulse_count_right); int pulse_count_difference_left = pulse_count_left - last_pulse_count_left_; @@ -511,7 +514,8 @@ void Raspimouse::calculate_odometry_from_pulse_counts(double & x, double & y, do return; } - RCLCPP_DEBUG(get_logger(), "Pulse differences: %d, %d", pulse_count_difference_left, + RCLCPP_DEBUG( + get_logger(), "Pulse differences: %d, %d", pulse_count_difference_left, pulse_count_difference_right); // Calculate number of revolutions since last time @@ -522,7 +526,8 @@ void Raspimouse::calculate_odometry_from_pulse_counts(double & x, double & y, do auto left_distance = left_revolutions * one_revolution_distance_left; auto right_distance = right_revolutions * one_revolution_distance_right; auto average_distance = (right_distance + left_distance) / 2; - RCLCPP_DEBUG(get_logger(), "Left dist: %f\tRight dist: %f\tAverage: %f", + RCLCPP_DEBUG( + get_logger(), "Left dist: %f\tRight dist: %f\tAverage: %f", left_distance, right_distance, average_distance); theta += atan2(right_distance - left_distance, wheel_base);