1、前置准备
缺少一些库需要提前安装,也可以边colcon边装,缺啥装啥,只作为记录
pip3 install lark -i https://pypi.doubanio.com/simple/pip3 install numpy -i https://pypi.doubanio.com/simple/pip3 uninstall em (如果有em的话 就卸载 安装empy)
pip3 install empy==3.3.2 -i https://pypi.doubanio.com/simple/ 版本号有要求
2、创建msg文件
qhr_ws/src/test_moudle 路径下创建msg文件夹
文件夹下创建文件 MyMessage.msg (文件名称必须首单词大写且没有下划线,类似my_message都会报错)
内容如下:
string name
int32 age
创建后目录树如上
3、修改依赖
CMakeLists.txt 添加
find_package(rosidl_default_generators REQUIRED)# 设置消息文件路径
set(msg_FILES"msg/MyMessage.msg"
)# 添加消息生成目标
rosidl_generate_interfaces(${PROJECT_NAME}${msg_FILES}
)# 安装消息文件
install(DIRECTORY msg/
DESTINATION share/${PROJECT_NAME}/msg
)
package.xml 添加
<depend>rosidl_default_generators</depend><exec_depend>rosidl_default_runtime</exec_depend><member_of_group>rosidl_interface_packages</member_of_group>
4、编译生成msg头文件
(rospy38) ➜ qhr_ws colcon build
Starting >>> test_moudle
Finished <<< test_moudle [7.44s] Summary: 1 package finished [7.60s](rospy38) ➜ qhr_ws source install/setup.zsh
(rospy38) ➜ qhr_ws ros2 interface show test_moudle/msg/MyMessage
string name
int32 age
(rospy38) ➜ qhr_ws
可以看到路径:install/test_moudle/include/test_moudle/test_moudle/msg/my_message.h 已经存在对应的头文件,并且通过命令行 也能查到对应的接口
5、talker和listener代码测试
发布code:
#include "rclcpp/rclcpp.hpp"
#include "test_moudle/msg/my_message.hpp" // 包含自定义消息头文件
#include <chrono>
#include <memory>using namespace std::chrono_literals;class Talker : public rclcpp::Node
{
public:Talker() : Node("talker"){publisher_ = this->create_publisher<test_moudle::msg::MyMessage>("my_topic", 10);auto publish_msg = [this]() -> void {auto message = test_moudle::msg::MyMessage();message.name = "Alice";message.age = count_++;RCLCPP_INFO(this->get_logger(), "Publishing: name='%s', age=%d", message.name.c_str(), message.age);this->publisher_->publish(message);};timer_ = this->create_wall_timer(500ms, publish_msg);}private:rclcpp::TimerBase::SharedPtr timer_;rclcpp::Publisher<test_moudle::msg::MyMessage>::SharedPtr publisher_;size_t count_ = 1;
};int main(int argc, char* argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<Talker>());rclcpp::shutdown();return 0;
}
订阅code:
#include "rclcpp/rclcpp.hpp"
#include "test_moudle/msg/my_message.hpp" // 包含自定义消息头文件
#include <memory>class Listener : public rclcpp::Node
{
public:Listener() : Node("listener"){subscription_ = this->create_subscription<test_moudle::msg::MyMessage>("my_topic", 10, std::bind(&Listener::topic_callback, this, std::placeholders::_1));}private:void topic_callback(const test_moudle::msg::MyMessage::SharedPtr msg) const{RCLCPP_INFO(this->get_logger(), "I heard: name='%s', age=%d", msg->name.c_str(), msg->age);}rclcpp::Subscription<test_moudle::msg::MyMessage>::SharedPtr subscription_;
};int main(int argc, char* argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<Listener>());rclcpp::shutdown();return 0;
}
修改cmakelists.txt
find_package(test_moudle REQUIRED)add_executable(talker src/talker.cpp)
ament_target_dependencies(talker rclcpp std_msgs test_moudle)add_executable(listener src/listener.cpp)
ament_target_dependencies(listener rclcpp std_msgs test_moudle)install(TARGETStalker listenerDESTINATION lib/${PROJECT_NAME})
编译msg要跟编译node区分开,要不find就出问题 编译不过,试了几种方法不行
6、测试
7、还有个问题
如果不生成py的msg 执行 echo会崩???