begin adding openpose ros node

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

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

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 -->

src/openpose_ros_node.cpp

+//#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