Added ROS talker class.

aicp_ros/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS aicp_core
                                         eigen_conversions
                                         pcl_conversions
                                         tf_conversions
+                                        std_srvs
                                         std_msgs
                                         sensor_msgs
                                         geometry_msgs
@@ -27,6 +28,7 @@ catkin_package(
                  eigen_conversions
                  pcl_conversions
                  tf_conversions
+                 std_srvs
                  std_msgs
                  sensor_msgs
                  geometry_msgs
@@ -41,6 +43,7 @@ include_directories(
 
 add_library(${PROJECT_NAME} SHARED src/app_ros.cpp
                                    src/visualizer_ros.cpp
+                                   src/talker_ros.cpp
                                    src/velodyne_accumulator.cpp)
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})

aicp_ros/include/aicp_ros/talker_ros.hpp

+#pragma once
+
+#include "ros/node_handle.h"
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/PoseArray.h>
+
+namespace aicp {
+
+class ROSTalker
+{
+public:
+
+    ROSTalker(ros::NodeHandle& nh, std::string fixed_frame);
+
+    // Publish footstep plan
+    void publishFootstepPlan(std::vector<Eigen::Isometry3d> &path,
+                             int64_t utime);
+
+private:
+    ros::NodeHandle& nh_;
+    ros::Publisher footstep_plan_pub_;
+    std::string fixed_frame_; // map or map_test
+};
+}

aicp_ros/package.xml

@@ -17,6 +17,7 @@
   <build_depend>eigen_conversions</build_depend>
   <build_depend>pcl_conversions</build_depend>
   <build_depend>tf_conversions</build_depend>
+  <build_depend>std_srvs</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
@@ -27,6 +28,7 @@
   <run_depend>eigen_conversions</run_depend>
   <run_depend>pcl_conversions</run_depend>
   <run_depend>tf_conversions</run_depend>
+  <run_depend>std_srvs</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>

aicp_ros/src/talker_ros.cpp

+#include "aicp_ros/talker_ros.hpp"
+
+//#include <eigen_conversions/eigen_msg.h>
+
+namespace aicp {
+
+ROSTalker::ROSTalker(ros::NodeHandle& nh, std::string fixed_frame) : nh_(nh),
+                                                                fixed_frame_(fixed_frame)
+{
+    footstep_plan_pub_ = nh_.advertise<geometry_msgs::PoseArray>("/position_controller/footstep_plan_request_list",10);
+}
+
+void ROSTalker::publishFootstepPlan(std::vector<Eigen::Isometry3d>& path,
+                                    int64_t utime){
+
+    geometry_msgs::PoseArray path_msg;
+    int secs = utime*1E-6;
+    int nsecs = (utime - (secs*1E6))*1E3;
+    path_msg.header.stamp = ros::Time(secs, nsecs);
+    path_msg.header.frame_id = fixed_frame_;
+
+    for (size_t i = 0; i < path.size(); ++i){
+        geometry_msgs::Pose p;
+        p.position.x = path[i].translation().x();
+        p.position.y = path[i].translation().y();
+        p.position.z = path[i].translation().z();
+        Eigen::Matrix3d pose_rot = path[i].rotation();
+        Eigen::Quaterniond pose_rot_q(pose_rot);
+        p.orientation.x = pose_rot_q.x();
+        p.orientation.y = pose_rot_q.y();
+        p.orientation.z = pose_rot_q.z();
+        p.orientation.w = pose_rot_q.w();
+
+        path_msg.poses.push_back(p);
+    }
+
+    footstep_plan_pub_.publish(path_msg);
+}
+}