首页 > 编程语言 >ros 自定义消息(图像+标志位+位姿)python和c++发布和接受

ros 自定义消息(图像+标志位+位姿)python和c++发布和接受

时间:2024-09-13 17:15:00浏览次数:11  
标签:msgs 自定义 python image pose c++ flag msg self

 

 

编译

 

脚本

 v3_gaosi_img_pose_flag.sh

#!/bin/bash
#外部给与执行权限
#sudo chmod +x run_ros_nodes.sh
# 定义 ROS 安装路径  #安装时候添加到系统路径了 不需要每次都source
ROS_SETUP="/opt/ros/noetic/setup.bash" 
# 定义工作目录路径  自己的工程没有加到系统路径,每次需要source
WORKSPACE_DIR="/home/r9000k/v2_project/gaosi_slam/ros/ros_cgg"


conda_envs="/home/r9000k/anaconda3" # 修改2-1 自己的conda 安装路径
#ROS_cv_briage_dir="/home/r9000k/v1_software/opencv/catkin_ws_cv_bridge/devel/setup.bash" # 修改2-2 自己编译的cv_briage包节点,貌似不用也行 制定了依赖opencv3.4.9 而非自带4.2
#echo $ROS_cv_briage_dir
conda_envs_int=$conda_envs"/etc/profile.d/conda.sh" # 不用改 conda自带初始化文件
echo $conda_envs_int
conda_envs_bin=$conda_envs"/envs/gaussian_splatting/bin" # 不用改 conda自带python安装位置 脚本中需要指定是conda特定的环境python而不是系统默认的
echo $conda_envs_bin
ROS_SETUP="/opt/ros/noetic/setup.bash" #不用改  安装时候添加到系统路径了 不需要每次都source 这里留着


#指定目录

# 启动 ROS Master
echo "Starting ROS 总结点..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
roscore; \
exec bash"

# 等待 ROS Master 启动
sleep 3

# 运行 C++ 发布节点
# echo "Running C++ 发布节点..."
# gnome-terminal -- bash -c "\
# cd $WORKSPACE_DIR; source devel/setup.bash; \
# rosrun gaosi_img_pose_flag image_pose_flag_publisher; \
# exec bash"

# echo "Running C++ 订阅节点..."
# gnome-terminal -- bash -c "\
# cd $WORKSPACE_DIR; source devel/setup.bash; \
# rosrun gaosi_img_pose_flag image_pose_flag_subscriber; \
# exec bash"


# 运行python节点
python_DIR="${WORKSPACE_DIR}/src/gaosi_img_pose_flag/src"

echo "Running python image_pose_publisher发布节点..."
gnome-terminal -- bash -c " \
cd $WORKSPACE_DIR; source devel/setup.bash; \
source $conda_envs_int; \ 
conda activate gaussian_splatting ; \
cd $python_DIR; \
/usr/bin/python3 image_pose_publisher.py; \
exec bash"


# 运行python节点

echo "Running python image_pose_subscriber订阅节点..."
gnome-terminal -- bash -c " \
cd $WORKSPACE_DIR; source devel/setup.bash; \
source $conda_envs_int; \ 
conda activate gaussian_splatting ; \
cd $python_DIR; \
/usr/bin/python3 image_pose_subscriber.py; \
exec bash"

  

 

1 创建自定义消息

 

 

ImagePose.msg
# ImagePose.msg
sensor_msgs/Image image
std_msgs/Float64 flag
geometry_msgs/Pose pose

  

CMakeLists.txt

 

 

cmake_minimum_required(VERSION 3.0.2)
project(gaosi_img_pose_flag)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  geometry_msgs
  sensor_msgs
  cv_bridge
  message_filters # 消息同步
  image_transport
  std_msgs # 自定义消息
  message_generation # 自定义消息
)

# 自定义消息
add_message_files(
  FILES
  ImagePose.msg
)

# 自定义消息
generate_messages(
  DEPENDENCIES
  std_msgs
  sensor_msgs
  geometry_msgs
)

find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(Eigen3 REQUIRED)

catkin_package(
  CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs cv_bridge std_msgs message_runtime
  DEPENDS Boost
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  "/usr/local/include/eigen3"
)



# 编译发布节点
add_executable(image_pose_flag_publisher src/image_pose_flag_publisher.cpp)
# 自定义消息引用
add_dependencies(image_pose_flag_publisher ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(image_pose_flag_publisher
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  ${Boost_LIBRARIES}
)

add_executable(image_pose_flag_subscriber src/image_pose_flag_subscriber.cpp)
add_dependencies(image_pose_flag_subscriber ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(image_pose_flag_subscriber ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})

  

 package.xml

<?xml version="1.0"?>
<package format="2">
  <name>gaosi_img_pose_flag</name>
  <version>0.0.1</version>
  <description>
    A package to publish and subscribe to images and GPS data using ROS.
  </description>

  <!-- Maintainer of the package -->
  <maintainer email="[email protected]">Your Name</maintainer>

  <!-- License of the package -->
  <license>MIT</license>

  <!-- Build tool required to build this package -->
  <buildtool_depend>catkin</buildtool_depend>

  <!-- Dependencies of the package during build and runtime -->
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>eigen</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>message_filters</build_depend>
  <build_depend>image_transport</build_depend>
  <!--自定义消息 -->
  <build_depend>message_generation</build_depend>
  <build_depend>message_runtime</build_depend>
  <build_depend>std_msgs</build_depend>
  
  
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>eigen</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>message_filters</exec_depend>
  <exec_depend>image_transport</exec_depend>
  <!--自定义消息 -->
  <exec_depend>message_generation</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <!-- Declare additional dependencies required for building this package -->
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>eigen</build_export_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>message_filters</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
    <!--自定义消息 -->
  <build_export_depend>message_generation</build_export_depend>
  <build_export_depend>message_runtime</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

  <!-- Export information, can be used by other packages -->
  <export>
    <!-- Export any specific information here -->
  </export>
</package>

  

 

 image_pose_publisher.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Float64
from geometry_msgs.msg import Pose
from gaosi_img_pose_flag.msg import ImagePose  # 更换为你包的名字
from cv_bridge import CvBridge
import numpy as np
import cv2

class ImagePosePublisher:
    def __init__(self):
        # Initialize node
        rospy.init_node('image_pose_publisher', anonymous=True)

        # Initialize publishers
        self.pub = rospy.Publisher('image_pose_topic', ImagePose, queue_size=10)

        # Create a bridge between OpenCV and ROS
        self.bridge = CvBridge()

        # Create or load a sample image
        self.image = np.zeros((480, 640, 3), dtype=np.uint8)  # Black image

        # Set flag
        self.flag = Float64()
        self.flag.data = 1.23  # Example double value

        # Set pose
        self.pose = Pose()
        self.pose.position.x = 1.0
        self.pose.position.y = 2.0
        self.pose.position.z = 3.0
        self.pose.orientation.x = 0.0
        self.pose.orientation.y = 0.0
        self.pose.orientation.z = 0.0
        self.pose.orientation.w = 1.0

        # Set publish rate
        self.rate = rospy.Rate(10)  # 10 Hz

        self.publish()

    def publish(self):
        while not rospy.is_shutdown():
            msg = ImagePose()

            # Convert OpenCV image to ROS image message
            msg.image = self.bridge.cv2_to_imgmsg(self.image, encoding="bgr8")
            msg.flag = self.flag
            msg.pose = self.pose

            # Publish message
            self.pub.publish(msg)

            self.rate.sleep()

if __name__ == '__main__':
    try:
        ImagePosePublisher()
    except rospy.ROSInterruptException:
        pass

  image_pose_subscriber.py

 

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import Float64
from geometry_msgs.msg import Pose
from gaosi_img_pose_flag.msg import ImagePose  # 更换为你包的名字
from cv_bridge import CvBridge
import cv2

class ImagePoseSubscriber:
    def __init__(self):
        # Initialize node
        rospy.init_node('image_pose_subscriber', anonymous=True)

        # Initialize subscriber
        self.sub = rospy.Subscriber('image_pose_topic', ImagePose, self.callback)

        # Create a bridge between OpenCV and ROS
        self.bridge = CvBridge()

        # Create an OpenCV window
        cv2.namedWindow("Received Image")

    def callback(self, msg):
        try:
            # Convert ROS image message to OpenCV image
            cv_image = self.bridge.imgmsg_to_cv2(msg.image, desired_encoding="bgr8")
            cv2.imshow("Received Image", cv_image)
            cv2.waitKey(30)
        except Exception as e:
            rospy.logerr("Failed to convert image: %s", str(e))

        rospy.loginfo("Received flag: %.2f", msg.flag.data)
        rospy.loginfo("Received pose: Position(%.2f, %.2f, %.2f), Orientation(%.2f, %.2f, %.2f, %.2f)",
                      msg.pose.position.x, msg.pose.position.y, msg.pose.position.z,
                      msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    try:
        subscriber = ImagePoseSubscriber()
        subscriber.run()
    except rospy.ROSInterruptException:
        pass

  

 

image_pose_flag_publisher.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Pose.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <gaosi_img_pose_flag/ImagePose.h> // 更换为你包的名字

class ImagePosePublisher
{
public:
    ImagePosePublisher()
    {
        // Initialize publisher
        pub_ = nh_.advertise<gaosi_img_pose_flag::ImagePose>("image_pose_topic", 10);

        // Load or create a sample image
        image_ = cv::Mat::zeros(480, 640, CV_8UC3); // Black image

        // Set flag
        flag_.data = 1.23; // Example double value

        // Set pose
        pose_.position.x = 1.0;
        pose_.position.y = 2.0;
        pose_.position.z = 3.0;
        pose_.orientation.x = 0.0;
        pose_.orientation.y = 0.0;
        pose_.orientation.z = 0.0;
        pose_.orientation.w = 1.0;

        // Set publish rate
        ros::Rate loop_rate(10); // 10 Hz

        while (ros::ok())
        {
            gaosi_img_pose_flag::ImagePose msg;

            // Convert OpenCV image to ROS image message
            msg.image = *cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_).toImageMsg();
            msg.flag = flag_;
            msg.pose = pose_;

            // Publish message
            pub_.publish(msg);

            ros::spinOnce();
            loop_rate.sleep();
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Publisher pub_;

    cv::Mat image_;
    std_msgs::Float64 flag_;
    geometry_msgs::Pose pose_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_pose_publisher");
    ImagePosePublisher image_pose_publisher;
    return 0;
}

  

image_pose_flag_subscriber.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Pose.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <gaosi_img_pose_flag/ImagePose.h> // 更换为你包的名字

class ImagePoseSubscriber
{
public:
    ImagePoseSubscriber()
    {
        // Initialize subscriber
        sub_ = nh_.subscribe("image_pose_topic", 10, &ImagePoseSubscriber::callback, this);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;

    void callback(const gaosi_img_pose_flag::ImagePose::ConstPtr& msg)
    {
        try
        {
            // Convert ROS image message to OpenCV image
            cv::Mat cv_image = cv_bridge::toCvCopy(msg->image, "bgr8")->image;
            cv::imshow("Received Image", cv_image);
            cv::waitKey(30);
        }
        catch (cv_bridge::Exception& e)
        {
            ROS_ERROR("cv_bridge exception: %s", e.what());
        }

        ROS_INFO("Received flag: %.2f", msg->flag.data);
        ROS_INFO("Received pose: Position(%.2f, %.2f, %.2f), Orientation(%.2f, %.2f, %.2f, %.2f)",
                 msg->pose.position.x, msg->pose.position.y, msg->pose.position.z,
                 msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "image_pose_subscriber");
    ImagePoseSubscriber image_pose_subscriber;
    ros::spin();
    return 0;
}

  

标签:msgs,自定义,python,image,pose,c++,flag,msg,self
From: https://www.cnblogs.com/gooutlook/p/18412553

相关文章

  • python处理时间字符串
    时间格式ISO8601标准"2024-09-11T00:00:03Z"Z的时间字符串表示UTC时间(协调世界时)。Z(Zerooffset/UTC)如果没有Z,通常还可以使用时区偏移来表示时间。例如:2024-09-11T00:00:03+08:00表示东八区的时间(比UTC提前8小时)。2024-09-11T00:00:03-05:00表示比UTC晚5小时的......
  • Python爬虫案例七:抓取南京公交信息数据并将其保存成excel多表形式
    测试链接:        https://nanjing.8684.cn/line4 思路:先抓取某个类型下的某一条线路所有数据,然后实现批量,,列举出三个类型代表既可源码:fromlxmlimportetreefromxlutils.copyimportcopyimportrequests,os,xlrd,xlwtdefget_all():#获取所......
  • Python 课程6-Pandas 和 Matplotlib库
    前言        在数据科学和数据分析领域,Pandas和Matplotlib是两个最常用的Python库。Pandas主要用于数据处理和分析,而Matplotlib则用于数据的可视化。它们的结合能够帮助我们快速、直观地展示数据的趋势和规律。在这篇详细的教程中,我将为你介绍Pandas和Matp......
  • 用命令检查自己电脑安装了哪些版本的python
    用命令检查自己电脑安装了哪些版本的pythonWindows:打开命令提示符(CommandPrompt)。输入以下命令:wherepython这将显示Python的安装路径,如果有多个版本,都会列出。你还可以使用py命令来查看所有已安装的Python版本:py-0这将列出所有可用的Python版本和......
  • 学Python需要用到哪些软件?
    Python作为一种功能全面且易于上手的编程语言,在数据科学、机器学习、web开发、数据分析等多个领域大放异彩。而为了帮助开发者更高效的编写Python代码,市面上也出现了许多优秀的Python软件,那么Python需要用到哪些软件?具体请看下文。要进行Python开发,需要以下软件:1、文......
  • C++--模板
    1泛型编程如何将Swap实现乘成一个通用的交换函数voidSwap(int&left,int&right){inttemp=left;left=right;right=temp;}voidSwap(double&left,double&right){doubletemp=left;left=right;right=temp;}voidSwap......
  • 邮政EMS查询|通过python查询快递单号API
    快递聚合查询的优势1、高效整合多种快递信息。2、实时动态更新。3、自动化管理流程。聚合国内外1500家快递公司的物流信息查询服务,使用API接口查询邮政EMS物流的便捷步骤,首先选择专业的数据平台的快递API接口:https://www.tanshuapi.com/market/detail-68以下示例是参考的示例代码:im......
  • Python 之records教程
    目录Python之records教程一、安装二、初始化三、增,删,改,查1.增加2.删除(必须使用事务,不然不生效)3.修改(必须使用事务,不然不生效)4.查询Python之records教程一、安装pipinstallrecords二、初始化importrecords#初始化db连接,支持从环境变量DATABASE_URL读取url......
  • echarts X轴文本太长 formatter自定义文本的显示方式
    如果ECharts中X轴的文本太长,可以通过设置axisLabel的rotate属性来旋转标签,或者使用formatter函数来自定义文本的显示方式。另外,可以开启axisLabel的interval属性来控制显示的标签的间隔。option={tooltip:{},xAxis:{type:'category',data:['这是一段非......
  • 学习Python如何选择合适的学校?
    近年来,由于Python语言比较火爆,掀起了一大波学习热潮,为了满足市场所需,Python培训机构应运而生。而在众多培训机构之中,选择一家适合且靠谱的机构很关键,那么学Python哪个机构好?具体请看下文。一些Python培训机构课程体系看起来很专业,但课程内容差强人意,因为老师讲课水平有限,......