Posenet ROS - arieldo/MorBot GitHub Wiki
In this tutorial, we are going to create a ROS package for incorporate body pose estimation using the poseNet model from jetson_inference
- JetBot environment set up Jetson Nano and Linux Setup
- ROS environment set up on your jetson , IF not Check out HowTo ROS
- Basic knowledge of ROS concepts, such as nodes, topics, and messages ,IF not Check out ROS 101
- Basic knowledge of Publisher and Subscriber in Python for ROS ,IF not Check out ROS PubSubPy
- Basic knowledge of Creating new Packages in ROS
- Jetbot Camera publishing package
- Basic knowledge of Python programming language
- Create a new package named
posenet
inside your workspace directory (~/MorBot/morbot_ros/morbot_ws/src
):
cd ~/MorBot/morbot_ros/morbot_ws/src
catkin_create_pkg posenet rospy sensor_msgs vision_msgs std_msgs geometry_msgs message_generation
- Create a new directory named
scripts
inside your package directory (~/MorBot/morbot_ros/morbot_ws/src/posenet
):
cd ~/MorBot/morbot_ros/morbot_ws/src/posenet
mkdir scripts
- Create a new source file inside the
scripts
directory. For example, create a Python file namedposenet_ros_node.py
:
cd scripts
touch posenet_ros_node.py
Make the script executable:
cd ~/MorBot/morbot_ros/morbot_ws/src/posenet/scripts
chmod +x posenet_ros_node.py
- The
posenet_ros_node.py
#!/usr/bin/env python
import sys
import rospy
import cv2
import jetson.utils
from jetson_utils import videoSource, videoOutput, Log
from jetson_inference import poseNet
from cv_bridge import CvBridge , CvBridgeError
from sensor_msgs.msg import Image
from posenet.msg import Pose2DArray # Import the custom message type
from geometry_msgs.msg import Point
bridge = CvBridge()
rospy.init_node('pose_estimation_node', anonymous=True)
pub_overlay = rospy.Publisher("/pose_estimations/overlay", Image, queue_size=1)
pub_poses = rospy.Publisher("/pose_estimations/info", Pose2DArray, queue_size=1) # Publish custom message
net = poseNet("resnet18-body", threshold=0.40)
def image_callback(msg):
global net
try:
cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
return
cv2_img = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2RGBA)
cuda_img = jetson.utils.cudaFromNumpy(cv2_img)
detections = net.Process(cuda_img, overlay="links,keypoints")
numpy_img = jetson.utils.cudaToNumpy(cuda_img)
overlay_img_msg = bridge.cv2_to_imgmsg(numpy_img, encoding="rgba8")
pose_msg = convert_detections_to_pose_msg(detections) # Convert detections to pose message
pub_overlay.publish(overlay_img_msg)
pub_poses.publish(pose_msg) # Publish the custom message
def convert_detections_to_pose_msg(detections):
pose_array_msg = Pose2DArray()
for detection in detections:
for keypoint, name in zip(detection.Keypoints, ["nose", "left_eye", "right_eye", "left_ear", "right_ear", "left_shoulder", "right_shoulder", "left_elbow", "right_elbow", "left_wrist", "right_wrist", "left_hip", "right_hip", "left_knee", "right_knee", "left_ankle", "right_ankle"]): # Assuming these are the keypoints you have, modify as needed
pt = Point(x=keypoint.x, y=keypoint.y, z=0) # z is set to 0 since it's a 2D keypoint
pose_array_msg.keypoints.append(pt)
pose_array_msg.keypoint_names.append(name)
return pose_array_msg
def main(args):
rospy.Subscriber("/camera/image_raw", Image, image_callback)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down ROS pose estimation module")
if __name__ == '__main__':
main(sys.argv)
- Define the Custom Message Type:
cd ~/MorBot/morbot_ros/morbot_ws/src/posenet
mkdir msg
cd msg
touch Pose2DArray.msg
- The
Pose2DArray.msg
# Pose2DArray.msg
geometry_msgs/Point[] keypoints # Array of 2D keypoints
string[] keypoint_names # Names of the keypoints (like "elbow", "knee", etc.)
- Configuring
CMakeLists.txt
for Custom ROS Messages :
To properly integrate custom messages into your ROS package, certain configurations in the CMakeLists.txt
file are essential. This ensures that your message files are recognized and correctly processed during the build process.
Follow these streamlined steps to ensure your custom messages are seamlessly integrated into your ROS package:
-
Open the Configuration
CMakeLists.txt
file in your package directory (~/MorBot/morbot_ros/morbot_ws/src/posenet
):-
Launch your preferred text editor and open the
CMakeLists.txt
file from your ROS package directory. -
Locate Message Declarations:
Scroll or search for the section that resembles:
## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # )
-
Declare Your Custom Message:
- Uncomment the
add_message_files()
function and add your custom message file to the list of messages to be generated. For example, if your custom message file is namedPose2DArray.msg
, the section should resemble:
## Generate messages in the 'msg' folder add_message_files( FILES Pose2DArray.msg )
- Uncomment the
-
Find Message Dependencies Section:
- Continue scrolling or search for a section similar to:
## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # geometry_msgs# sensor_msgs# std_msgs# vision_msgs # )
-
Specify Message Dependencies:
- Uncomment the
generate_messages()
function and add your message dependencies to the list. For example, if your custom message file depends on thegeometry_msgs
,sensor_msgs
,std_msgs
, andvision_msgs
packages, the section should resemble:
generate_messages( DEPENDENCIES geometry_msgs sensor_msgs std_msgs vision_msgs )
- Uncomment the
-
-
By following these steps, you ensure that your custom message Pose2DArray.msg and its dependencies will be recognized and processed correctly during the ROS package build.
-
Configuring
package.xml
for Custom ROS Messages :-
Open the Configuration
package.xml
file in your package directory (~/MorBot/morbot_ros/morbot_ws/src/posenet
):Launch your preferred text editor and open the package.xml file from your ROS package directory.
-
Locate Message Declarations:
Scroll or search for the section that resembles
message_runtime
:<exec_depend>message_runtime</exec_depend>
-
Uncomment by deleting the
<!-- and -->
the above section .
-
- Build the package:
cd ~/MorBot/morbot_ros/morbot_ws
catkin_make
source devel/setup.bash
- Create a new directory named
launch
inside your package directory (~/MorBot/morbot_ros/morbot_ws/src/posenet
):
cd ~/MorBot/morbot_ros/morbot_ws/src/posenet
mkdir launch
- Create a new launch file inside the
launch
directory. For example, create a launch file namedposenet_ros.launch
:
cd launch
touch posenet_ros.launch
- The
posenet_ros.launch
<launch>
<!-- Start the camera publisher -->
<include file="$(find camera_publish)/launch/camera_publisher.launch" />
<!-- Start the posenet node node -->
<node name="posenet_node" pkg="posenet" type="posenet_ros_node.py" output="screen" />
</launch>
- Run the launch file:
roslaunch posenet posenet_ros.launch
-
Open a web browser and navigate to
http://<jetbot_ip_address>:8080/stream?topic=/pose_estimations/overlay
to view the camerapose_estimations
-
Check the published images:
rostopic echo /pose_estimations/info
That's it! You have created a ROS package for object detection using poseNet.