diff --git a/CMakeLists.txt b/CMakeLists.txt
index 352bc394f0bf9f977f91fd9a40a1bfa32df53b2e..8842a85fe58200cc7a04c329c45306de0ea4dd19 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,16 +3,18 @@ project(openpose_ros)
set(OPENPOSE_INCLUDE_DIRS openpose/include
openpose/src
- /usr/local/cuda/include
- openpose/3rdparty/caffe/distribute/include)
+ openpose/3rdparty/caffe/distribute/include
+ /usr/local/cuda/include)
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
-std_msgs
-roscpp)
+ std_msgs
+ roscpp
+ cv_bridge
+)
###################################
## catkin specific configuration ##
@@ -28,6 +30,7 @@ catkin_package(
# LIBRARIES openpose_ros
CATKIN_DEPENDS std_msgs
roscpp
+ cv_bridge
# DEPENDS system_lib
)
@@ -54,11 +57,14 @@ target_link_libraries(${PROJECT_NAME}_talker_node ${catkin_LIBRARIES}
${CAFFE_LIBRARY}
${CUDA_LIBRARY})
-add_executable(${PROJECT_NAME}_openpose_node src/openpose.cpp)
-target_link_libraries(${PROJECT_NAME}_openpose_node ${catkin_LIBRARIES}
+add_executable(${PROJECT_NAME} src/openpose.cpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}
${OPENPOSE_LIBRARY}
${CAFFE_LIBRARY}
${CUDA_LIBRARY}
${GFLAGS_LIBRARY}
${GLOG_LIBRARY})
+add_executable(openpose_ros_node src/openpose_ros_node.cpp)
+target_link_libraries(openpose_ros_node ${catkin_LIBRARIES}
+ ${OPENPOSE_LIBRARY})
\ No newline at end of file
diff --git a/README.md b/README.md
index d557722f6e673151f3b6fcf041c45bc635109507..fa186493f7d57bcc7bda9859d33723b1f3212b0b 100644
--- a/README.md
+++ b/README.md
@@ -6,7 +6,6 @@
2. If everything succeeds, run `catkin build`, source your workspace then:
````
roscd openpose_ros/openpose
-rosrun openpose_ros openpose_ros_openpose_node --image_dir examples/media/
+rosrun openpose_ros openpose_ros --image_dir examples/media/
````
-How silly of me. Somebody already did this with a rosservice call. Check it out: https://github.com/tue-robotics/openpose_ros
\ No newline at end of file
diff --git a/package.xml b/package.xml
index 743d905ebda86f2ba23e70e5825196e2e8164188..6f72f94984a55175787527077c3fbc1bd3b94987 100644
--- a/package.xml
+++ b/package.xml
@@ -43,9 +43,11 @@
<build_depend>std_msgs</build_depend>
<build_depend>roscpp</build_depend>
+ <build_depend>cv_bridge</build_depend>
<run_depend>std_msgs</run_depend>
<run_depend>roscpp</run_depend>
+ <run_depend>cv_bridge</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
diff --git a/src/openpose_ros_node.cpp b/src/openpose_ros_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1b61f06747ddad5d6e8060a8e3e77e4b73d2d988
--- /dev/null
+++ b/src/openpose_ros_node.cpp
@@ -0,0 +1,252 @@
+//#define USE_CAFFE
+#include <openpose/pose/poseExtractor.hpp>
+#include <openpose/pose/poseExtractorCaffe.hpp>
+#include <openpose/pose/poseParameters.hpp>
+
+#include <openpose/core/headers.hpp>
+#include <openpose/filestream/headers.hpp>
+#include <openpose/gui/headers.hpp>
+#include <openpose/pose/headers.hpp>
+#include <openpose/utilities/headers.hpp>
+
+#include <std_srvs/Empty.h>
+#include <ros/node_handle.h>
+#include <ros/service_server.h>
+#include <ros/init.h>
+#include <cv_bridge/cv_bridge.h>
+
+//#include <image_recognition_msgs/GetPersons.h>
+
+std::shared_ptr<op::PoseExtractor> g_pose_extractor;
+std::map<unsigned char, std::string> g_bodypart_map;
+cv::Size g_net_input_size;
+unsigned int g_num_scales;
+double g_scale_gap;
+
+/*
+
+//!
+//! \brief getParam Get parameter from node handle
+//! \param nh The nodehandle
+//! \param param_name Key string
+//! \param default_value Default value if not found
+//! \return The parameter value
+//!
+template <typename T>
+T getParam(const ros::NodeHandle& nh, const std::string& param_name, T default_value)
+{
+ T value;
+ if (nh.hasParam(param_name))
+ {
+ nh.getParam(param_name, value);
+ }
+ else
+ {
+ ROS_WARN_STREAM("Parameter '" << param_name << "' not found, defaults to '" << default_value << "'");
+ value = default_value;
+ }
+ return value;
+}
+
+op::PoseModel stringToPoseModel(const std::string& pose_model_string)
+{
+ if (pose_model_string == "COCO")
+ return op::PoseModel::COCO_18;
+ else if (pose_model_string == "MPI")
+ return op::PoseModel::MPI_15;
+ else if (pose_model_string == "MPI_4_layers")
+ return op::PoseModel::MPI_15_4;
+ else
+ {
+ ROS_ERROR("String does not correspond to any model (COCO, MPI, MPI_4_layers)");
+ return op::PoseModel::COCO_18;
+ }
+}
+
+std::map<unsigned char, std::string> getBodyPartMapFromPoseModel(const op::PoseModel& pose_model)
+{
+ if (pose_model == op::PoseModel::COCO_18)
+ {
+ return op::POSE_COCO_BODY_PARTS;
+ }
+ else if (pose_model == op::PoseModel::MPI_15 || pose_model == op::PoseModel::MPI_15_4)
+ {
+ return op::POSE_MPI_BODY_PARTS;
+ }
+ else
+ {
+ ROS_FATAL("Invalid pose model, not map present");
+ exit(1);
+ }
+}
+
+image_recognition_msgs::BodypartDetection getBodyPartDetectionFromArrayAndIndex(const op::Array<float>& array, size_t idx)
+{
+ image_recognition_msgs::BodypartDetection bodypart;
+ bodypart.x = array[idx];
+ bodypart.y = array[idx+1];
+ bodypart.confidence = array[idx+2];
+ return bodypart;
+}
+
+image_recognition_msgs::BodypartDetection getNANBodypart()
+{
+ image_recognition_msgs::BodypartDetection bodypart;
+ bodypart.confidence = NAN;
+ return bodypart;
+}
+
+bool detectPosesCallback(image_recognition_msgs::GetPersons::Request& req, image_recognition_msgs::GetPersons::Response& res)
+{
+ ROS_INFO("detectPosesCallback");
+
+ // Convert ROS message to opencv image
+ cv_bridge::CvImagePtr cv_ptr;
+ try
+ {
+ cv_ptr = cv_bridge::toCvCopy(req.image, req.image.encoding);
+ }
+ catch (cv_bridge::Exception& e)
+ {
+ ROS_ERROR("detectPosesCallback cv_bridge exception: %s", e.what());
+ return false;
+ }
+ cv::Mat image = cv_ptr->image;
+
+ ROS_INFO_STREAM("Perform forward pass with the following settings:");
+ ROS_INFO_STREAM("- net_input_size: " << g_net_input_size);
+ ROS_INFO_STREAM("- num_scales: " << g_num_scales);
+ ROS_INFO_STREAM("- scale_gap: " << g_scale_gap);
+ ROS_INFO_STREAM("- image_size: " << image.size());
+ op::CvMatToOpInput cv_mat_to_op_input(g_net_input_size, g_num_scales, g_scale_gap);
+
+ g_pose_extractor->forwardPass(cv_mat_to_op_input.format(image), image.size());
+ ROS_INFO("g_pose_extractor->forwardPass done");
+
+ const op::Array<float> poses;
+ if (!poses.empty() && poses.getNumberDimensions() != 3)
+ {
+ ROS_ERROR("pose.getNumberDimensions(): %d != 3", (int) poses.getNumberDimensions());
+ return false;
+ }
+
+ int num_people = poses.getSize(0);
+ int num_bodyparts = poses.getSize(1);
+
+ for (size_t person_idx = 0; person_idx < num_people; person_idx++)
+ {
+ // Initialize all bodyparts with nan
+ image_recognition_msgs::PersonDetection person_msg;
+ person_msg.nose = getNANBodypart();
+ person_msg.neck = getNANBodypart();
+ person_msg.right_shoulder = getNANBodypart();
+ person_msg.right_elbow = getNANBodypart();
+ person_msg.right_wrist = getNANBodypart();
+ person_msg.left_shoulder = getNANBodypart();
+ person_msg.left_elbow = getNANBodypart();
+ person_msg.left_wrist = getNANBodypart();
+ person_msg.right_hip = getNANBodypart();
+ person_msg.right_knee = getNANBodypart();
+ person_msg.right_ankle = getNANBodypart();
+ person_msg.left_hip = getNANBodypart();
+ person_msg.left_knee = getNANBodypart();
+ person_msg.left_ankle = getNANBodypart();
+ person_msg.right_eye = getNANBodypart();
+ person_msg.left_eye = getNANBodypart();
+ person_msg.right_ear = getNANBodypart();
+ person_msg.left_ear = getNANBodypart();
+ person_msg.chest = getNANBodypart();
+
+ for (size_t bodypart_idx = 0; bodypart_idx < num_bodyparts; bodypart_idx++)
+ {
+ size_t final_idx = 3*(person_idx*num_bodyparts + bodypart_idx);
+ image_recognition_msgs::BodypartDetection bodypart_detection = getBodyPartDetectionFromArrayAndIndex(poses, final_idx);
+
+ std::string body_part_string = g_bodypart_map[bodypart_idx];
+
+ if (body_part_string == "Nose") person_msg.nose = bodypart_detection;
+ else if (body_part_string == "Neck") person_msg.neck = bodypart_detection;
+ else if (body_part_string == "RShoulder") person_msg.right_shoulder = bodypart_detection;
+ else if (body_part_string == "RElbow") person_msg.right_elbow = bodypart_detection;
+ else if (body_part_string == "RWrist") person_msg.right_wrist = bodypart_detection;
+ else if (body_part_string == "LShoulder") person_msg.left_shoulder = bodypart_detection;
+ else if (body_part_string == "LElbow") person_msg.left_elbow = bodypart_detection;
+ else if (body_part_string == "LWrist") person_msg.left_wrist = bodypart_detection;
+ else if (body_part_string == "RHip") person_msg.right_hip = bodypart_detection;
+ else if (body_part_string == "RKnee") person_msg.right_knee = bodypart_detection;
+ else if (body_part_string == "RAnkle") person_msg.right_ankle = bodypart_detection;
+ else if (body_part_string == "LHip") person_msg.left_hip = bodypart_detection;
+ else if (body_part_string == "LKnee") person_msg.left_knee = bodypart_detection;
+ else if (body_part_string == "LAnkle") person_msg.left_ankle = bodypart_detection;
+ else if (body_part_string == "REye") person_msg.right_eye = bodypart_detection;
+ else if (body_part_string == "LEye") person_msg.left_eye = bodypart_detection;
+ else if (body_part_string == "REar") person_msg.right_ear = bodypart_detection;
+ else if (body_part_string == "LEar") person_msg.left_ear = bodypart_detection;
+ else if (body_part_string == "Chest") person_msg.chest = bodypart_detection;
+ else
+ {
+ ROS_ERROR("Unknown bodypart %s, this should never happen!", body_part_string.c_str());
+ }
+ }
+ res.detections.push_back(person_msg);
+ }
+
+ ROS_INFO("Detected %d persons", (int) res.detections.size());
+
+ return true;
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "image_recognition_msgs");
+
+ ros::NodeHandle local_nh("~");
+ g_net_input_size = cv::Size(getParam(local_nh, "net_input_width", 656), getParam(local_nh, "net_input_height", 368));
+ cv::Size net_output_size(getParam(local_nh, "net_output_width", 656), getParam(local_nh, "net_output_height", 368));
+ cv::Size output_size(getParam(local_nh, "output_width", 1280), getParam(local_nh, "output_height", 720));
+ g_num_scales = getParam(local_nh, "num_scales", 1);
+ g_scale_gap = getParam(local_nh, "scale_gap", 0.3);
+ unsigned int num_gpu_start = getParam(local_nh, "num_gpu_start", 0);
+ std::string model_folder = getParam(local_nh, "model_folder", std::string("~/openpose/models"));
+ op::PoseModel pose_model = stringToPoseModel(getParam(local_nh, "pose_model", std::string("COCO")));
+ g_bodypart_map = getBodyPartMapFromPoseModel(pose_model);
+
+ ros::NodeHandle nh;
+ ros::ServiceServer service = nh.advertiseService("detect_poses", detectPosesCallback);
+
+ g_pose_extractor = std::shared_ptr<op::PoseExtractorCaffe>(
+ new op::PoseExtractorCaffe(g_net_input_size, net_output_size, output_size, g_num_scales,
+ g_scale_gap, pose_model, model_folder, num_gpu_start));
+
+ ros::spin();
+
+ return 0;
+}
+*/
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "image_recognition_msgs");
+
+/*
+ ros::NodeHandle local_nh("~");
+ g_net_input_size = cv::Size(getParam(local_nh, "net_input_width", 656), getParam(local_nh, "net_input_height", 368));
+ cv::Size net_output_size(getParam(local_nh, "net_output_width", 656), getParam(local_nh, "net_output_height", 368));
+ cv::Size output_size(getParam(local_nh, "output_width", 1280), getParam(local_nh, "output_height", 720));
+ g_num_scales = getParam(local_nh, "num_scales", 1);
+ g_scale_gap = getParam(local_nh, "scale_gap", 0.3);
+ unsigned int num_gpu_start = getParam(local_nh, "num_gpu_start", 0);
+ std::string model_folder = getParam(local_nh, "model_folder", std::string("~/openpose/models"));
+ op::PoseModel pose_model = stringToPoseModel(getParam(local_nh, "pose_model", std::string("COCO")));
+ g_bodypart_map = getBodyPartMapFromPoseModel(pose_model);
+
+ ros::NodeHandle nh;
+ ros::ServiceServer service = nh.advertiseService("detect_poses", detectPosesCallback);
+
+ g_pose_extractor = std::shared_ptr<op::PoseExtractorCaffe>(
+ new op::PoseExtractorCaffe(g_net_input_size, net_output_size, output_size, g_num_scales,
+ g_scale_gap, pose_model, model_folder, num_gpu_start));
+*/
+ ros::spin();
+
+ return 0;
+}
\ No newline at end of file