Add DumpRosbagFromBvh.cpp

Author: mmurooka

Diff for: CMakeLists.txt

@@ -20,12 +20,13 @@ project(${PROJECT_NAME} LANGUAGES CXX)
 # RBDyn
 find_package(RBDyn REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS roscpp roslib sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS roscpp roslib rosbag sensor_msgs)
 
 catkin_package(
   CATKIN_DEPENDS
   roscpp
   roslib
+  rosbag
   sensor_msgs
   INCLUDE_DIRS
   include

Diff for: README.md

@@ -17,16 +17,21 @@ $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo
 
 ## Examples
 
+### Visualize BVH animation
+```bash
+$ roslaunch mocap_ros_utils visualize_bvh.launch
+```
+
+https://github.com/isri-aist/MocapRosUtils/assets/6636600/c1c3b077-3f81-42dd-9d78-1073588b167a
+
 ### Dump URDF file from BVH hierarchy data
 ```bash
 $ rosrun mocap_ros_utils GenerateUrdfFromBvh `rospack find mocap_ros_utils`/data/sample_walk.bvh /tmp/BvhRobot.urdf BvhRobot
 # Visualize URDF model
 $ roslaunch urdf_tutorial display.launch model:=/tmp/BvhRobot.urdf
 ```
 
-### Visualize BVH motion with TF
+### Dump rosbag file from BVH motion data
 ```bash
-$ roslaunch mocap_ros_utils visualize_bvh.launch
+$ rosrun mocap_ros_utils DumpRosbagFromBvh `rospack find mocap_ros_utils`/data/sample_walk.bvh /tmp/sample_walk.bag
 ```
-
-https://github.com/isri-aist/MocapRosUtils/assets/6636600/c1c3b077-3f81-42dd-9d78-1073588b167a

Diff for: include/MocapRosUtils/JointTrajData.h

@@ -31,6 +31,11 @@ class JointTrajData
    */
   void publishJointState(bool loop = false) const;
 
+  /** \brief Dump joint state message to a rosbag file.
+      \param rosbag_filename Path of the rosbag file
+   */
+  void dumpRosbag(const std::string & rosbag_filename) const;
+
 public:
   //! Sampling period [sec]
   double dt_ = 0;

Diff for: node/CMakeLists.txt

@@ -1,4 +1,5 @@
-set(MocapRosUtils_node_list GenerateUrdfFromBvh PublishJointStateFromBvh)
+set(MocapRosUtils_node_list GenerateUrdfFromBvh PublishJointStateFromBvh
+                            DumpRosbagFromBvh)
 
 foreach(NAME IN LISTS MocapRosUtils_node_list)
   add_executable(${NAME} ${NAME}.cpp)

Diff for: node/DumpRosbagFromBvh.cpp

@@ -0,0 +1,29 @@
+/* Author: Masaki Murooka */
+
+#include <MocapRosUtils/BvhConverter.h>
+
+using namespace MocapRosUtils;
+
+int main(int argc, char ** argv)
+{
+  if(argc != 3)
+  {
+    std::cout << "Usage:\n  " << argv[0] << " <bvh_file_path> <rosbag_file_path>" << std::endl;
+    return 1;
+  }
+  std::string bvh_filename = argv[1];
+  std::string rosbag_filename = argv[2];
+
+  BvhConverter::Configuration bvh_converter_config;
+  bvh_converter_config.print_level = 0;
+  bvh_converter_config.pos_scale = 1e-2;
+  bvh_converter_config.urdf_cylinder_radius_list = {{"Spine", 0.03}, {"Spine1", 0.03}, {"Head", 0.1}};
+  bvh_converter_config.disable_end_link_visual = false;
+
+  BvhConverter bvh_converter(bvh_filename, bvh_converter_config);
+
+  ros::Time::init();
+  bvh_converter.joint_traj_data_.dumpRosbag(rosbag_filename);
+
+  return 0;
+}

Diff for: node/GenerateUrdfFromBvh.cpp

@@ -21,7 +21,7 @@ int main(int argc, char ** argv)
   bvh_converter_config.print_level = 1;
   bvh_converter_config.pos_scale = 1e-2;
   bvh_converter_config.urdf_cylinder_radius_list = {{"Spine", 0.03}, {"Spine1", 0.03}, {"Head", 0.1}};
-  bvh_converter_config.disable_end_link_visual = true;
+  bvh_converter_config.disable_end_link_visual = false;
 
   BvhConverter bvh_converter(bvh_filename, bvh_converter_config);
 

Diff for: node/PublishJointStateFromBvh.cpp

@@ -13,7 +13,7 @@ int main(int argc, char ** argv)
   bvh_converter_config.print_level = 0;
   bvh_converter_config.pos_scale = 1e-2;
   bvh_converter_config.urdf_cylinder_radius_list = {{"Spine", 0.03}, {"Spine1", 0.03}, {"Head", 0.1}};
-  bvh_converter_config.disable_end_link_visual = true;
+  bvh_converter_config.disable_end_link_visual = false;
 
   std::string bvh_filename;
   nh.getParam("bvh_filename", bvh_filename);

Diff for: package.xml

@@ -14,6 +14,7 @@
 
   <depend>roscpp</depend>
   <depend>roslib</depend>
+  <depend>rosbag</depend>
   <depend>sensor_msgs</depend>
 
   <exec_depend>tf2_ros</exec_depend>

Diff for: src/JointTrajData.cpp

@@ -4,6 +4,8 @@
 
 #include <ros/ros.h>
 
+#include <rosbag/bag.h>
+
 using namespace MocapRosUtils;
 
 sensor_msgs::JointState JointTrajData::makeJointStateMsg(int frame_idx) const
@@ -50,3 +52,16 @@ void JointTrajData::publishJointState(bool loop) const
     }
   }
 }
+
+void JointTrajData::dumpRosbag(const std::string & rosbag_filename) const
+{
+  rosbag::Bag bag;
+  bag.open(rosbag_filename, rosbag::bagmode::Write);
+
+  for(int frame_idx = 0; frame_idx < pos_list_.rows(); frame_idx++)
+  {
+    bag.write("joint_states", ros::Time::now(), makeJointStateMsg(frame_idx));
+  }
+
+  bag.close();
+}