Skip to content

Commit

Permalink
Update for foxy (#29)
Browse files Browse the repository at this point in the history
* Change rosidl_generator_cpp to rosidl_runtime_cpp to fix build error

* Use ament_export_targets instead of ament_export_interfaces

* update industrial_ci for foxy

* refactoring to pass the uncrustify test
  • Loading branch information
Shota Aoki authored Dec 7, 2020
1 parent 51569c8 commit 78e8816
Show file tree
Hide file tree
Showing 3 changed files with 17 additions and 12 deletions.
2 changes: 1 addition & 1 deletion .github/workflows/industrial_ci.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion raspimouse/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
25 changes: 15 additions & 10 deletions raspimouse/src/raspimouse_component.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
#include <limits>
#include <memory>
#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;
Expand All @@ -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),
Expand Down Expand Up @@ -118,12 +118,14 @@ CallbackReturn Raspimouse::on_configure(const rclcpp_lifecycle::State &)
light_sensors_pub_ = this->create_publisher<raspimouse_msgs::msg::LightSensors>(
"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<raspimouse_msgs::msg::Leds>(
Expand Down Expand Up @@ -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_;
Expand All @@ -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
Expand All @@ -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);
Expand Down

0 comments on commit 78e8816

Please sign in to comment.