基于ROS Kinetic的CV2脚本地面坐标Marker消息发送实现咨询

Got it, let's walk through adding the ROS Marker publishing feature to your existing code. Since you're working with Python 2.7 and ROS Kinetic, here's a step-by-step implementation that fits right into your current setup:

Step 1: Add Required ROS Imports #

First, drop these ROS Python modules at the top of your script (right after your OpenCV imports):

import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point

Step 2: Initialize ROS Node & Marker Publisher #

Add this code right after loading the homographic matrix (right below the print "Homographic matrix loaded successfully." line):

# Initialize ROS node (anonymous=True ensures a unique node name)
rospy.init_node('goal_point_publisher', anonymous=True)

# Create a publisher for Marker messages - adjust the topic name if needed
marker_pub = rospy.Publisher('/goal_point_marker', Marker, queue_size=10)

# Set the fixed frame (update this to match your robot's TF tree, e.g., "odom" or "map")
# Most mobile robots use "odom" as the local reference frame
FRAME_ID = "odom"

Tip: If you're unsure which frame to use, start with "odom" — you can switch to "map" later if you have a SLAM system running.

Step 3: Build & Publish Marker in the Mouse Callback #

Modify your draw_circle2 function to create and send the Marker message as soon as you calculate pointOut. Add this code right after the print "Current Location: "+str(pointOut) line:

# Create a new Marker message
goal_marker = Marker()
goal_marker.header.frame_id = FRAME_ID
goal_marker.header.stamp = rospy.Time.now()

# Set a unique ID for the marker (use different IDs if you want multiple markers)
goal_marker.id = 0

# Choose marker type (SPHERE is easy to spot; you can use ARROW, CUBE, etc. too)
goal_marker.type = Marker.SPHERE

# Tell RViz to add/update this marker
goal_marker.action = Marker.ADD

# Populate the marker's position with your ground coordinates
goal_marker.pose.position.x = pointOut[0]
goal_marker.pose.position.y = pointOut[1]
goal_marker.pose.position.z = 0.0  # Assuming your ground plane is at z=0

# No rotation needed for a sphere, so set default orientation
goal_marker.pose.orientation.x = 0.0
goal_marker.pose.orientation.y = 0.0
goal_marker.pose.orientation.z = 0.0
goal_marker.pose.orientation.w = 1.0

# Set marker size (adjust based on your robot's scale; 0.2m is good for small robots)
goal_marker.scale.x = 0.2
goal_marker.scale.y = 0.2
goal_marker.scale.z = 0.2

# Set marker color (RGBA format, values 0-1; this is bright red)
goal_marker.color.r = 1.0
goal_marker.color.g = 0.0
goal_marker.color.b = 0.0
goal_marker.color.a = 1.0  # Alpha = 1 means fully opaque

# Publish the marker to RViz
marker_pub.publish(goal_marker)

Step 4: Update the Main Loop for ROS Events #

Your main while loop needs to process ROS events to ensure messages get published. Add rospy.spin_once() at the start of the loop:

while True: # making a loop
    rospy.spin_once()  # Add this line to handle ROS publishing/subscription events
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
    if (cv2.waitKey(1) & 0xFF == ord('c')): #Deleting points from image
        cv2.imshow('Video',emptyFrame) #Show the image again, deleting all graphical overlays like text and shapes
        coord_on_screen = [] #Resetting coordinate lists
        coord_on_ground=[] #Resetting coordinate lists
        # Optional: Publish a delete action to remove the marker from RViz
        clear_marker = Marker()
        clear_marker.id = 0
        clear_marker.action = Marker.DELETE
        marker_pub.publish(clear_marker)
    if (cv2.waitKey(1) & 0xFF == ord('s')):
        cap.release()
        cv2.destroyAllWindows()
        init()

Step 5: Test It Out #

1. Start your ROS core with roscore in a terminal.
2. Make your script executable: chmod +x your_script.py
3. Run your script, then open RViz.
4. In RViz, add a "Marker" display and set its topic to /goal_point_marker.
5. Double-click on your camera feed to select a point — you should see a red sphere pop up at the corresponding ground coordinate in RViz!

Quick Tweaks You Might Want: #

• If you want a different marker style, change goal_marker.type to Marker.ARROW, Marker.CUBE, or another valid type.
• Adjust the scale values to make the marker bigger/smaller based on your robot's size.
• If you press 'c' and want the marker to disappear, use the optional delete code I added in the 'c' branch.

内容的提问来源于stack exchange，提问作者Michael