Commit 509e8875 authored by simalpha's avatar simalpha
Browse files

Added ROS talker class.

parent aa1c9c04
......@@ -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})
......
#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
};
}
......@@ -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>
......
#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);
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment