Skip to content

Commit

Permalink
Merge pull request #196 from orikuma/add-ground-truth-ros-bridge
Browse files Browse the repository at this point in the history 
Add ground truth ros bridge
  • Loading branch information
@YoheiKakiuchi
YoheiKakiuchi authored Jul 25, 2016
2 parents cd7c522 + 6955c44 commit 31f5912
Show file tree
Hide file tree
Showing 5 changed files with 518 additions and 0 deletions.
4 changes: 4 additions & 0 deletions hrpsys_choreonoid/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,10 @@ if(${cnoid-plugin_FOUND})
link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${cnoid-plugin_LIBRARY_DIRS} ${Boost_LIBRARY_DIRS} ${hrpsys_LIBRARY_DIRS})
add_executable(JointStateROSBridge src/JointStateROSBridge.cpp src/JointStateROSBridgeComp.cpp)
target_link_libraries(JointStateROSBridge ${openrtm_aist_LIBRARIES} ${cnoid-plugin_LIBRARIES} ${Boost_LIBRARIES} ${hrpsys_LIBRARIES} ${catkin_LIBRARIES})

add_executable(GroundTruthROSBridge src/GroundTruthROSBridge.cpp src/GroundTruthROSBridgeComp.cpp)
target_link_libraries(GroundTruthROSBridge ${openrtm_aist_LIBRARIES} ${cnoid-plugin_LIBRARIES} ${Boost_LIBRARIES} ${hrpsys_LIBRARIES} ${catkin_LIBRARIES})

endif()

## Build only choreonoid iob
Expand Down
269 changes: 269 additions & 0 deletions hrpsys_choreonoid/src/GroundTruthROSBridge.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,269 @@
// -*- C++ -*-
/*!
* @file GroundTruthROSBridge.cpp * @brief openhrp image - ros bridge * $Date$
*
* $Id$
*/
#include "GroundTruthROSBridge.h"
#include <hrpUtil/EigenTypes.h>
#include <hrpUtil/Eigen3d.h>

#include "nav_msgs/Odometry.h"
#include "geometry_msgs/Twist.h"

// Module specification
// <rtc-template block="module_spec">
static const char* imagesensorrosbridge_spec[] =
{
"implementation_id", "GroundTruthROSBridge",
"type_name", "GroundTruthROSBridge",
"description", "publish ground truth from choreonoid",
"version", "1.0",
"vendor", "JSK",
"category", "example",
"activity_type", "SPORADIC",
"kind", "DataFlowComponent",
"max_instance", "10",
"language", "C++",
"lang_type", "compile",
// Configuration variables
""
};
// </rtc-template>

GroundTruthROSBridge::GroundTruthROSBridge(RTC::Manager* manager)
// <rtc-template block="initializer">
: RTC::DataFlowComponentBase(manager),
m_baseTformIn("baseTform", m_baseTform),
prev_stamp(ros::Time(0)),
is_initialized(false),
pub_cycle(0)
{
}

GroundTruthROSBridge::~GroundTruthROSBridge()
{
}


RTC::ReturnCode_t GroundTruthROSBridge::onInitialize()
{
// Registration: InPort/OutPort/Service
// <rtc-template block="registration">
// Set InPort buffers
addInPort("baseTform", m_baseTformIn);

// Set OutPort buffer

// Set service provider to Ports

// Set service consumers to Ports

// Set CORBA Service Ports

// </rtc-template>

// <rtc-template block="bind_config">
// Bind variables and configuration variable

// </rtc-template>
ground_truth_odom_pub = node.advertise<nav_msgs::Odometry>("ground_truth_odom", 1);

if(ros::param::has("~rate")) {
double rate;
ros::param::get("~rate", rate);
if(rate != 0) {
pub_cycle = 1/rate;
}
}

init_base.setOrigin(tf::Vector3(0, 0, 0));
init_base.setRotation(tf::Quaternion(0, 0, 0, 1));
prev_base.setOrigin(tf::Vector3(0, 0, 0));
prev_base.setRotation(tf::Quaternion(0, 0, 0, 1));

// initialize
ROS_INFO_STREAM("[GroundTruthROSBridge] @Initilize name : " << getInstanceName());

return RTC::RTC_OK;
}


/*
RTC::ReturnCode_t GroundTruthROSBridge::onFinalize()
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onStartup(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onShutdown(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onActivated(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onDeactivated(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/

RTC::ReturnCode_t GroundTruthROSBridge::onExecute(RTC::UniqueId ec_id)
{
//capture_time = ros::Time::now();

if (m_baseTformIn.isNew()) {
m_baseTformIn.read();

tf::Transform current_base;
ros::Time current_stamp = ros::Time(m_baseTform.tm.sec, m_baseTform.tm.nsec);
convertBaseTformToTfTransform(current_base);
double dt = (current_stamp - prev_stamp).toSec();

// skip for publish rate
if (pub_cycle != 0 && dt < pub_cycle) {
return RTC::RTC_OK;
}

// transform current_base into initial base relative coords
{
if (is_initialized == false) {
tf::Vector3 init_origin = current_base.getOrigin();
tf::Quaternion init_rotation = current_base.getRotation();
init_origin.setZ(0.0); // z origin should be on the ground
init_base.setOrigin(init_origin);
init_base.setRotation(init_rotation);
current_base = init_base.inverse() * current_base;
prev_base = current_base; // prev_base should be initilaized, too
is_initialized = true;
} else {
current_base = init_base.inverse() * current_base;
}
}

// calculate velocity
tf::Vector3 linear_twist, angular_twist;
calculateTwist(current_base, prev_base, linear_twist, angular_twist, dt);
geometry_msgs::Twist current_twist;
tf::vector3TFToMsg(linear_twist, current_twist.linear);
tf::vector3TFToMsg(angular_twist, current_twist.angular);

// register msg
nav_msgs::Odometry ground_truth_odom;
ground_truth_odom.header.stamp = current_stamp;
tf::Vector3 current_origin = current_base.getOrigin();
ground_truth_odom.pose.pose.position.x = current_origin[0];
ground_truth_odom.pose.pose.position.y = current_origin[1];
ground_truth_odom.pose.pose.position.z = current_origin[2];
tf::quaternionTFToMsg(current_base.getRotation(), ground_truth_odom.pose.pose.orientation);
ground_truth_odom.twist.twist = current_twist;

ground_truth_odom_pub.publish(ground_truth_odom);

// preserve values for velocity calculation in next step
prev_base = current_base;
prev_stamp = current_stamp;
}

return RTC::RTC_OK;
}

/*
RTC::ReturnCode_t GroundTruthROSBridge::onAborting(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onError(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onReset(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onStateUpdate(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/
/*
RTC::ReturnCode_t GroundTruthROSBridge::onRateChanged(RTC::UniqueId ec_id)
{
return RTC::RTC_OK;
}
*/



void GroundTruthROSBridge::convertBaseTformToTfTransform(tf::Transform& result_base)
{
// baseTform: x, y, z, R[0][0], R[0][1], ... , R[2][2]
double *data = m_baseTform.data.get_buffer();
tf::Vector3 base_origin(data[0], data[1], data[2]);
result_base.setOrigin(base_origin);
hrp::Matrix33 hrpsys_R;
hrp::getMatrix33FromRowMajorArray(hrpsys_R, data, 3);
hrp::Vector3 hrpsys_rpy = hrp::rpyFromRot(hrpsys_R);
tf::Quaternion base_q = tf::createQuaternionFromRPY(hrpsys_rpy(0), hrpsys_rpy(1), hrpsys_rpy(2));
result_base.setRotation(base_q);
return;
}

void GroundTruthROSBridge::calculateTwist(const tf::Transform& _current_base, const tf::Transform& _prev_base, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt)
{
// current rotation matrix
tf::Matrix3x3 current_basis = _current_base.getBasis();

// linear twist
tf::Vector3 current_origin = _current_base.getOrigin();
tf::Vector3 prev_origin = _prev_base.getOrigin();
_linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt);

// angular twist
// R = exp(omega_w*dt) * prev_R
// omega_w is described in global coordinates in relationships of twist transformation.
// it is easier to calculate omega using hrp functions than tf functions
tf::Matrix3x3 prev_basis = _prev_base.getBasis();
double current_rpy[3], prev_rpy[3];
current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]);
prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]);
hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt;
_angular_twist.setX(hrp_omega[0]);
_angular_twist.setY(hrp_omega[1]);
_angular_twist.setZ(hrp_omega[2]);

return;
}

extern "C"
{

void GroundTruthROSBridgeInit(RTC::Manager* manager)
{
coil::Properties profile(imagesensorrosbridge_spec);
manager->registerFactory(profile,
RTC::Create<GroundTruthROSBridge>,
RTC::Delete<GroundTruthROSBridge>);
}

};
Loading

0 comments on commit 31f5912

Please sign in to comment.