Skip to content

Commit

Permalink
Humble migration
Browse files Browse the repository at this point in the history
Signed-off-by: Francisco Martín Rico <[email protected]>
  • Loading branch information
fmrico committed Aug 8, 2022
1 parent 62cea9e commit 24a48a5
Show file tree
Hide file tree
Showing 9 changed files with 20 additions and 27 deletions.
6 changes: 3 additions & 3 deletions br2_basics/src/param_reader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,9 @@ class LocalizationNode : public rclcpp::Node
public:
LocalizationNode() : Node("localization_node")
{
declare_parameter<int>("number_particles", 200);
declare_parameter<std::vector<std::string>>("topics", {});
declare_parameter<std::vector<std::string>>("topic_types", {});
declare_parameter("number_particles", 200);
declare_parameter("topics", std::vector<std::string>());
declare_parameter("topic_types", std::vector<std::string>());

get_parameter("number_particles", num_particles_);
RCLCPP_INFO_STREAM(get_logger(), "Number of particles: " << num_particles_);
Expand Down
1 change: 0 additions & 1 deletion br2_bt_bumpgo/tests/bt_action_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,6 @@
#include "sensor_msgs/msg/laser_scan.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

#include "gtest/gtest.h"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ class BtLifecycleCtrlNode : public BT::ActionNodeBase
} else {
if (state != ctrl_node_state_) {
RCLCPP_ERROR(
node_->get_logger(), "Transition not possible %zu -> %zu", ctrl_node_state_, state);
node_->get_logger(), "Transition not possible %u -> %u", ctrl_node_state_, state);
return false;
} else {
return true;
Expand All @@ -155,10 +155,10 @@ class BtLifecycleCtrlNode : public BT::ActionNodeBase

if (!result.get()->success) {
RCLCPP_ERROR(
node_->get_logger(), "Failed to set node state %zu -> %zu", ctrl_node_state_, state);
node_->get_logger(), "Failed to set node state %u -> %u", ctrl_node_state_, state);
return false;
} else {
RCLCPP_INFO(node_->get_logger(), "Transition success %zu -> %zu", ctrl_node_state_, state);
RCLCPP_INFO(node_->get_logger(), "Transition success %u -> %u", ctrl_node_state_, state);
}

ctrl_node_state_ = state;
Expand Down
2 changes: 2 additions & 0 deletions br2_tf2_detector/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,13 +8,15 @@ find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(visualization_msgs REQUIRED)

set(dependencies
rclcpp
tf2_ros
geometry_msgs
tf2_geometry_msgs
sensor_msgs
visualization_msgs
)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@

#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <memory>

Expand All @@ -23,6 +22,8 @@
#include "sensor_msgs/msg/laser_scan.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

#include "rclcpp/rclcpp.hpp"

namespace br2_tf2_detector
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,15 @@

#include <tf2/transform_datatypes.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <memory>

#include "br2_tf2_detector/ObstacleMonitorNode.hpp"

#include "geometry_msgs/msg/transform_stamped.hpp"

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"

#include "rclcpp/rclcpp.hpp"

namespace br2_tf2_detector
Expand Down
5 changes: 2 additions & 3 deletions br2_tracking/src/br2_tracking/ObjectDetector.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,9 +83,8 @@ ObjectDetector::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & m
detection_msg.header = msg->header;
detection_msg.bbox.size_x = bbx.width;
detection_msg.bbox.size_y = bbx.height;
detection_msg.bbox.center.x = cx;
detection_msg.bbox.center.y = cy;
detection_msg.source_img = *cv_ptr->toImageMsg();
detection_msg.bbox.center.position.x = cx;
detection_msg.bbox.center.position.y = cy;
detection_pub_->publish(detection_msg);

if (debug_) {
Expand Down
7 changes: 5 additions & 2 deletions br2_tracking/src/object_tracker_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,14 +30,17 @@ int main(int argc, char * argv[])
auto node_head_controller = std::make_shared<br2_tracking::HeadController>();
auto node_tracker = rclcpp::Node::make_shared("tracker");

const int IMG_WIDTH = 640;
const int IMG_HEIGHT = 480;

auto command_pub = node_tracker->create_publisher<br2_tracking_msgs::msg::PanTiltCommand>(
"/command", 100);
auto detection_sub = node_tracker->create_subscription<vision_msgs::msg::Detection2D>(
"/detection", rclcpp::SensorDataQoS(),
[command_pub](vision_msgs::msg::Detection2D::SharedPtr msg) {
br2_tracking_msgs::msg::PanTiltCommand command;
command.pan = (msg->bbox.center.x / msg->source_img.width) * 2.0 - 1.0;
command.tilt = (msg->bbox.center.y / msg->source_img.height) * 2.0 - 1.0;
command.pan = (msg->bbox.center.position.x / IMG_WIDTH) * 2.0 - 1.0;
command.tilt = (msg->bbox.center.position.y / IMG_HEIGHT) * 2.0 - 1.0;
command_pub->publish(command);
});

Expand Down
14 changes: 1 addition & 13 deletions third_parties.repos
Original file line number Diff line number Diff line change
@@ -1,16 +1,4 @@
repositories:
ThirdParty/gazebo_ros2_control:
type: git
url: https://github.com/pal-robotics-forks/gazebo_ros2_control.git
version: add-imu-sensor-parse
ThirdParty/ros2_control:
type: git
url: https://github.com/ros-controls/ros2_control.git
version: 0.7.1
ThirdParty/ros2_controllers:
type: git
url: https://github.com/ros-controls/ros2_controllers.git
version: 0.4.0
ThirdParty/urdf_test:
type: git
url: https://github.com/pal-robotics/urdf_test
Expand All @@ -34,7 +22,7 @@ repositories:
ThirdParty/twist_mux:
type: git
url: https://github.com/ros-teleop/twist_mux.git
version: foxy-devel
version: rolling
ThirdParty/tiago_robot:
type: git
url: https://github.com/fmrico/tiago_robot.git
Expand Down

0 comments on commit 24a48a5

Please sign in to comment.