Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Used turtlesim_msgs #78

Merged
merged 2 commits into from
Jul 10, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions turtle_tf2_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(turtlesim REQUIRED)
find_package(turtlesim_msgs REQUIRED)

if(TARGET tf2_geometry_msgs::tf2_geometry_msgs)
get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES)
Expand Down Expand Up @@ -52,7 +52,7 @@ ament_target_dependencies(
rclcpp
tf2
tf2_ros
turtlesim
turtlesim_msgs
)

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
Expand All @@ -62,7 +62,7 @@ ament_target_dependencies(
rclcpp
tf2
tf2_ros
turtlesim
turtlesim_msgs
)

add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
Expand Down
4 changes: 2 additions & 2 deletions turtle_tf2_cpp/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
<author email="abilkasov@gmail.com">Shyngyskhan Abilkassov</author>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>geometry_msgs</depend>
<depend>launch</depend>
<depend>launch_ros</depend>
Expand All @@ -22,7 +22,7 @@
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>turtlesim</depend>
<depend>turtlesim_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
8 changes: 4 additions & 4 deletions turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"
#include "turtlesim_msgs/msg/pose.hpp"

class FramePublisher : public rclcpp::Node
{
Expand All @@ -42,7 +42,7 @@ class FramePublisher : public rclcpp::Node
stream << "/" << turtlename_.c_str() << "/pose";
std::string topic_name = stream.str();

auto handle_turtle_pose = [this](const std::shared_ptr<turtlesim::msg::Pose> msg) {
auto handle_turtle_pose = [this](const std::shared_ptr<turtlesim_msgs::msg::Pose> msg) {
geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
Expand Down Expand Up @@ -70,13 +70,13 @@ class FramePublisher : public rclcpp::Node
// Send the transformation
tf_broadcaster_->sendTransform(t);
};
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
subscription_ = this->create_subscription<turtlesim_msgs::msg::Pose>(
topic_name, 10,
handle_turtle_pose);
}

private:
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
rclcpp::Subscription<turtlesim_msgs::msg::Pose>::SharedPtr subscription_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
std::string turtlename_;
};
Expand Down
12 changes: 6 additions & 6 deletions turtle_tf2_cpp/src/turtle_tf2_listener.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim/srv/spawn.hpp"
#include "turtlesim_msgs/srv/spawn.hpp"

using namespace std::chrono_literals;

Expand All @@ -45,7 +45,7 @@ class FrameListener : public rclcpp::Node

// Create a client to spawn a turtle
spawner_ =
this->create_client<turtlesim::srv::Spawn>("spawn");
this->create_client<turtlesim_msgs::srv::Spawn>("spawn");

// Create turtle2 velocity publisher
publisher_ =
Expand Down Expand Up @@ -102,16 +102,16 @@ class FrameListener : public rclcpp::Node
// Check if the service is ready
if (spawner_->service_is_ready()) {
// Initialize request with turtle name and coordinates
// Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
// Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn
auto request = std::make_shared<turtlesim_msgs::srv::Spawn::Request>();
request->x = 4.0;
request->y = 2.0;
request->theta = 0.0;
request->name = "turtle2";

// Call request
using ServiceResponseFuture =
rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
rclcpp::Client<turtlesim_msgs::srv::Spawn>::SharedFuture;
auto response_received_callback = [this](ServiceResponseFuture future) {
auto result = future.get();
if (strcmp(result->name.c_str(), "turtle2") == 0) {
Expand All @@ -132,7 +132,7 @@ class FrameListener : public rclcpp::Node
bool turtle_spawning_service_ready_;
// if the turtle was successfully spawned
bool turtle_spawned_;
rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
rclcpp::Client<turtlesim_msgs::srv::Spawn>::SharedPtr spawner_{nullptr};
rclcpp::TimerBase::SharedPtr timer_{nullptr};
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
Expand Down
4 changes: 3 additions & 1 deletion turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,9 @@ class PoseDrawer : public rclcpp::Node
tf2_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");
point_sub_.subscribe(
this, "/turtle3/turtle_point_stamped",
rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)));
tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
this->get_node_clock_interface(), buffer_timeout);
Expand Down
2 changes: 1 addition & 1 deletion turtle_tf2_py/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>turtlesim</exec_depend>
<exec_depend>turtlesim_msgs</exec_depend>

<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
Expand Down
2 changes: 1 addition & 1 deletion turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose
from turtlesim_msgs.msg import Pose


# This function is a stripped down version of the code in
Expand Down
2 changes: 1 addition & 1 deletion turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ def on_timer(self):
else:
if self.spawner.service_is_ready():
# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
# Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn
request = Spawn.Request()
request.name = 'turtle2'
request.x = 4.0
Expand Down
6 changes: 3 additions & 3 deletions turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,8 @@
import rclpy
from rclpy.node import Node

from turtlesim.msg import Pose
from turtlesim.srv import Spawn
from turtlesim_msgs.msg import Pose
from turtlesim_msgs.srv import Spawn


class PointPublisher(Node):
Expand Down Expand Up @@ -53,7 +53,7 @@ def on_timer(self):
else:
if self.spawner.service_is_ready():
# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
# Note that x, y and theta are defined as floats in turtlesim_msgs/srv/Spawn
request = Spawn.Request()
request.name = 'turtle3'
request.x = 4.0
Expand Down
Loading