diff --git a/hrpsys_choreonoid/CMakeLists.txt b/hrpsys_choreonoid/CMakeLists.txt index 64928892..04a90c21 100644 --- a/hrpsys_choreonoid/CMakeLists.txt +++ b/hrpsys_choreonoid/CMakeLists.txt @@ -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 diff --git a/hrpsys_choreonoid/src/GroundTruthROSBridge.cpp b/hrpsys_choreonoid/src/GroundTruthROSBridge.cpp new file mode 100644 index 00000000..e9d75dcd --- /dev/null +++ b/hrpsys_choreonoid/src/GroundTruthROSBridge.cpp @@ -0,0 +1,269 @@ +// -*- C++ -*- +/*! + * @file GroundTruthROSBridge.cpp * @brief openhrp image - ros bridge * $Date$ + * + * $Id$ + */ +#include "GroundTruthROSBridge.h" +#include +#include + +#include "nav_msgs/Odometry.h" +#include "geometry_msgs/Twist.h" + +// Module specification +// +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 + "" + }; +// + +GroundTruthROSBridge::GroundTruthROSBridge(RTC::Manager* manager) + // + : 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 + // + // Set InPort buffers + addInPort("baseTform", m_baseTformIn); + + // Set OutPort buffer + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + // + // Bind variables and configuration variable + + // + ground_truth_odom_pub = node.advertise("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, + RTC::Delete); + } + +}; diff --git a/hrpsys_choreonoid/src/GroundTruthROSBridge.h b/hrpsys_choreonoid/src/GroundTruthROSBridge.h new file mode 100644 index 00000000..1dc0f8ce --- /dev/null +++ b/hrpsys_choreonoid/src/GroundTruthROSBridge.h @@ -0,0 +1,146 @@ +// -*- C++ -*- +/*! + * @file GroundTruthROSBridge.h * @brief openhrp image - ros bridge * @date $Date$ + * + * $Id$ + */ +#ifndef GROUNDTRUTHROSBRIDGE_H +#define GROUNDTRUTHROSBRIDGE_H + +#include +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// +// + +// ros +#include "ros/ros.h" +#include + +using namespace RTC; + +class GroundTruthROSBridge : public RTC::DataFlowComponentBase +{ + public: + GroundTruthROSBridge(RTC::Manager* manager); + ~GroundTruthROSBridge(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + // virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + + TimedDoubleSeq m_baseTform; + InPort m_baseTformIn; + + // + + // DataOutPort declaration + // + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + ros::NodeHandle node; + ros::Publisher ground_truth_odom_pub; + + tf::Transform init_base; + tf::Transform prev_base; + ros::Time prev_stamp; + + double pub_cycle; + bool is_initialized; + + void convertBaseTformToTfTransform(tf::Transform& base); + void calculateTwist(const tf::Transform& _current_base, const tf::Transform& _prev_base, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt); + +}; + + +extern "C" +{ + DLL_EXPORT void GroundTruthROSBridgeInit(RTC::Manager* manager); +}; + +#endif // RANGESENSORROSBRIDGE_H diff --git a/hrpsys_choreonoid/src/GroundTruthROSBridgeComp.cpp b/hrpsys_choreonoid/src/GroundTruthROSBridgeComp.cpp new file mode 100644 index 00000000..68964b89 --- /dev/null +++ b/hrpsys_choreonoid/src/GroundTruthROSBridgeComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file GroundTruthROSBridgeComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ +#include +#include +#include +#include "GroundTruthROSBridge.h" + +void MyModuleInit(RTC::Manager* manager) +{ + GroundTruthROSBridgeInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("GroundTruthROSBridge"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference +// RTC::RTObject_var rtobj; +// rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component +// PortList* portlist; +// portlist = rtobj->get_ports(); + + // getting port profiles +// std::cout << "Number of Ports: "; +// std::cout << portlist->length() << std::endl << std::endl; +// for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) +// { +// Port_ptr port; +// port = (*portlist)[i]; +// std::cout << "Port" << i << " (name): "; +// std::cout << port->get_port_profile()->name << std::endl; +// +// RTC::PortInterfaceProfileList iflist; +// iflist = port->get_port_profile()->interfaces; +// std::cout << "---interfaces---" << std::endl; +// for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) +// { +// std::cout << "I/F name: "; +// std::cout << iflist[i].instance_name << std::endl; +// std::cout << "I/F type: "; +// std::cout << iflist[i].type_name << std::endl; +// const char* pol; +// pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; +// std::cout << "Polarity: " << pol << std::endl; +// } +// std::cout << "---properties---" << std::endl; +// NVUtil::dump(port->get_port_profile()->properties); +// std::cout << "----------------" << std::endl << std::endl; +// } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + ros::init(argc, argv, "GroundTruthROSBridgeComp", ros::init_options::NoSigintHandler); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} + diff --git a/hrpsys_choreonoid_tutorials/launch/jaxon_red_vision_connect.launch b/hrpsys_choreonoid_tutorials/launch/jaxon_red_vision_connect.launch index 15d6ef13..e47fab85 100644 --- a/hrpsys_choreonoid_tutorials/launch/jaxon_red_vision_connect.launch +++ b/hrpsys_choreonoid_tutorials/launch/jaxon_red_vision_connect.launch @@ -103,4 +103,12 @@ to="JointStateROSBridge0.rtc:qRef" /> + + + + +