编译
脚本
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