diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt index 57d701ea9..4469c9ad0 100644 --- a/hrpsys_ros_bridge/CMakeLists.txt +++ b/hrpsys_ros_bridge/CMakeLists.txt @@ -66,7 +66,7 @@ compile_collada_model(${collada_robots_PACKAGE_PATH}/data/robots/willowgarage-pr add_custom_command(OUTPUT ${webots_simulator_PACKAGE_PATH}/build/webots/resources/projects/robots/darwin-op/protos/DARwInOP.proto COMMAND make -C ${webots_simulator_PACKAGE_PATH}) add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/models/DARWIN.wrl - COMMAND rosrun roseus roseus ${PROJECT_SOURCE_DIR}/scripts/webotsvrml2openhrp3vrml.l + COMMAND rosrun roseus roseus ${PROJECT_SOURCE_DIR}/euslisp/webotsvrml2openhrp3vrml.l "\\(progn \\(convert-webots-vrml-\\>openhrp3-vrml-for-darwin \\\"${PROJECT_SOURCE_DIR}/models/DARWIN.wrl\\\"\\) \\(exit\\)\\)" DEPENDS ${webots_simulator_PACKAGE_PATH}/build/webots/resources/projects/robots/darwin-op/protos/DARwInOP.proto) if (EXISTS ${webots_simulator_PACKAGE_PATH}/build/webots/resources/projects/robots/darwin-op/protos/DARwInOP.proto) @@ -105,6 +105,7 @@ endforeach(compile_target ${compile_robots}) ## rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp) rtmbuild_add_executable(ImageSensorROSBridge src/ImageSensorROSBridge.cpp src/ImageSensorROSBridgeComp.cpp) +rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp) ## ## test diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp new file mode 100644 index 000000000..ed5e3ff3f --- /dev/null +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp @@ -0,0 +1,344 @@ +// -*- C++ -*- +/*! + * @file HrpsysJointTrajectoryBridge.cpp * @brief hrpsys setJointAngle - ros joint trajectory bridge * $Date$ + * + * $Id$ + */ +#include "HrpsysJointTrajectoryBridge.h" + +#include "rtm/idl/RTC.hh" +#include "hrpsys/idl/ExecutionProfileService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" + +// Module specification +// +static const char* hrpsysjointtrajectorybridge_spec[] = + { + "implementation_id", "HrpsysJointTrajectoryBridge", + "type_name", "HrpsysJointTrajectoryBridge", + "description", "hrpsys setJointAngle - ros joint trajectory bridge", + "version", "1.0", + "vendor", "Yohei Kakiuchi", + "category", "example", + "activity_type", "SPORADIC", + "kind", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "" + }; +// + +HrpsysJointTrajectoryBridge::HrpsysJointTrajectoryBridge(RTC::Manager* manager) + // + : RTC::DataFlowComponentBase(manager), + m_SequencePlayerServicePort("SequencePlayerService") + // +{ +} + +HrpsysJointTrajectoryBridge::~HrpsysJointTrajectoryBridge() +{ +} + + +RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onInitialize() +{ + m_SequencePlayerServicePort.registerConsumer("service0", "SequencePlayerService", m_service0); + + addPort(m_SequencePlayerServicePort); + + RTC::Properties& prop = getProperties(); + + body = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0) { + comPos = nameServer.length(); + } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + std::string modelfile = m_pManager->getConfig ()["model"]; + + if (!loadBodyFromModelLoader(body, modelfile.c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()))){ + std::cerr << "[HrpsysJointTrajectoryBridge] failed to load model[" << prop["model"] << "]" + << std::endl; + } + bool ret = false; + while ( ! ret ) { + try { + bodyinfo = hrp::loadBodyInfo(modelfile.c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext())); + ret = loadBodyFromBodyInfo(body, bodyinfo); + } catch ( CORBA::SystemException& ex ) { + std::cerr << "[HrpsysJointTrajectoryBridge] CORBA::SystemException " << ex._name() << std::endl; + sleep(1); + } catch ( ... ) { + std::cerr << "[HrpsysJointTrajectoryBridge] failed to load model[" << modelfile << "]" << std::endl;; + sleep(1); + } + } + + if (body == NULL) { + return RTC::RTC_ERROR; + } + body->calcForwardKinematics(); + + ROS_INFO_STREAM("[HrpsysJointTrajectoryBridge] @Initilize name : " << getInstanceName() << " done"); + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id) { + // ROS_INFO("ON_ACTIVATED"); + std::string config_name; + config_name = nh.resolveName("controller_configuration"); + if (nh.hasParam(config_name)) { + XmlRpc::XmlRpcValue param_val; + nh.getParam(config_name, param_val); + if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) { + for(int i = 0; i < param_val.size(); i++) { + if (param_val[i].getType() == XmlRpc::XmlRpcValue::TypeStruct) { + XmlRpc::XmlRpcValue gval = param_val[i]["group_name"]; + XmlRpc::XmlRpcValue cval = param_val[i]["controller_name"]; + XmlRpc::XmlRpcValue lval = param_val[i]["joint_list"]; + + std::string gname = gval; + std::string cname = cval; + std::vector jlst; + if (lval.getType() == XmlRpc::XmlRpcValue::TypeArray) { + for(int s = 0; s < lval.size(); s++) { + jlst.push_back(lval[s]); + } + } + if(gname.length() == 0 && cname.length() > 0) { + gname = cname; + } + if(gname.length() > 0 && cname.length() == 0) { + cname = gname; + } + if(gname.length() > 0 && cname.length() > 0 && jlst.size() > 0) { + ROS_INFO_STREAM("ADD_GROUP: " << gname << " (" << cname << ")"); + ROS_INFO_STREAM(" JOINT:"); + for(size_t s = 0; s < jlst.size(); s++) { + ROS_INFO_STREAM(" " << jlst[s]); + } + jointTrajectoryActionObj::Ptr tmp (new jointTrajectoryActionObj (this, cval, gval, jlst)); + trajectory_actions.push_back(tmp); + } + } + } + } else { + ROS_WARN_STREAM("param: " << config_name << ", configuration is not an array."); + } + } else { + ROS_WARN_STREAM("param: " << config_name << ", param does not exist."); + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onFinalize() +{ + // delete objs + for(size_t i = 0; i < trajectory_actions.size(); i++) { + trajectory_actions[i].reset(); + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onExecute(RTC::UniqueId ec_id) +{ + ros::spinOnce(); + for (size_t i = 0; i < trajectory_actions.size(); i++) { + trajectory_actions[i]->proc(); + } + + return RTC::RTC_OK; +} + +HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +jointTrajectoryActionObj(HrpsysJointTrajectoryBridge *ptr, + std::string &cname, std::string &gname, std::vector &jlist) { + parent = ptr; + controller_name = cname; + groupname = gname; + joint_list = jlist; + + joint_trajectory_server.reset + (new actionlib::SimpleActionServer + (parent->nh, controller_name + "/joint_trajectory_action", false)); + follow_joint_trajectory_server.reset + (new actionlib::SimpleActionServer + (parent->nh, controller_name + "/follow_joint_trajectory_action", false)); + + joint_trajectory_server-> + registerGoalCallback(boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj + ::onJointTrajectoryActionGoal, this)); + joint_trajectory_server-> + registerPreemptCallback(boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj + ::onJointTrajectoryActionPreempt, this)); + follow_joint_trajectory_server-> + registerGoalCallback(boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj + ::onFollowJointTrajectoryActionGoal, this)); + follow_joint_trajectory_server-> + registerPreemptCallback(boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj + ::onFollowJointTrajectoryActionPreempt, this)); + + joint_controller_state_pub = parent->nh. + advertise(controller_name + "/state", 1); + + if (groupname.length() > 0) { + OpenHRP::SequencePlayerService::StrSequence jnames; + jnames.length(joint_list.size()); + for(size_t i = 0; i < joint_list.size(); i++) { + jnames[i] = joint_list[i].c_str(); + } + parent->m_service0->addJointGroup(groupname.c_str(), jnames); + } + + interpolationp = false; + + joint_trajectory_server->start(); + follow_joint_trajectory_server->start(); +} + +HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::~jointTrajectoryActionObj() { + joint_trajectory_server->setPreempted(); + follow_joint_trajectory_server->setPreempted(); +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +proc() { + // finish interpolation + if (joint_trajectory_server->isActive() && + interpolationp == true && parent->m_service0->isEmpty() == true) { + pr2_controllers_msgs::JointTrajectoryResult result; + joint_trajectory_server->setSucceeded(result); + interpolationp = false; + } + if (follow_joint_trajectory_server->isActive() && + interpolationp == true && parent->m_service0->isEmpty() == true) { + control_msgs::FollowJointTrajectoryResult result; + result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + follow_joint_trajectory_server->setSucceeded(result); + interpolationp = false; + } +#if 0 + ros::Time tm_on_execute = ros::Time::now(); + + control_msgs::FollowJointTrajectoryFeedback follow_joint_trajectory_feedback; + follow_joint_trajectory_feedback.header.stamp = tm_on_execute; + + if (!follow_joint_trajectory_feedback->joint_names.empty() && + !follow_joint_trajectory_feedback->actual.positions.empty()) { + follow_joint_trajectory_server->publishFeedback(follow_joint_trajectory_feedback); + } +#endif +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +onJointTrajectory(trajectory_msgs::JointTrajectory trajectory) { + parent->m_mutex.lock(); + + ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ")"); + // TODO: check size and joint names + + OpenHRP::dSequenceSequence angles; + OpenHRP::dSequence duration; + + angles.length(trajectory.points.size()) ; + duration.length(trajectory.points.size()) ; + + std::vector joint_names = trajectory.joint_names; + + for (unsigned int i=0; i < trajectory.points.size(); i++) { + angles[i].length(parent->body->joints().size()); + + trajectory_msgs::JointTrajectoryPoint point = trajectory.points[i]; + for (unsigned int j=0; j < trajectory.joint_names.size(); j++ ) { + parent->body->link(joint_names[j])->q = point.positions[j]; + } + + parent->body->calcForwardKinematics(); + + int j = 0; + std::vector::const_iterator it = parent->body->joints().begin(); + while ( it != parent->body->joints().end() ) { + hrp::Link* l = ((hrp::Link*)*it); + angles[i][j] = l->q; + ++it;++j; + } + + ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : time_from_start " << trajectory.points[i].time_from_start.toSec()); + + if ( i > 0 ) { + duration[i] = trajectory.points[i].time_from_start.toSec() - trajectory.points[i-1].time_from_start.toSec(); + } else { // if i == 0 + if ( trajectory.points.size()== 1 ) { + duration[i] = trajectory.points[i].time_from_start.toSec(); + } else { // 0.2 is magic number writtein in roseus/euslisp/robot-interface.l + duration[i] = trajectory.points[i].time_from_start.toSec() - 0.2; + } + } + } + + parent->m_mutex.unlock(); + + if ( duration.length() == 1 ) { + if (groupname.length() > 0) { + // group + ROS_INFO_STREAM("setJointAnglesOfGroup: " << groupname); + parent->m_service0->setJointAnglesOfGroup (groupname.c_str(), angles[0], duration[0]); + } else { + // fullbody + parent->m_service0->setJointAngles(angles[0], duration[0]); + } + } else { + OpenHRP::dSequenceSequence rpy, zmp; + parent->m_service0->playPattern(angles, rpy, zmp, duration); + } + + interpolationp = true; +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +onJointTrajectoryActionGoal() { + pr2_controllers_msgs::JointTrajectoryGoalConstPtr + goal = joint_trajectory_server->acceptNewGoal(); + onJointTrajectory(goal->trajectory); +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +onFollowJointTrajectoryActionGoal() { + control_msgs::FollowJointTrajectoryGoalConstPtr + goal = follow_joint_trajectory_server->acceptNewGoal(); + onJointTrajectory(goal->trajectory); +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +onJointTrajectoryActionPreempt() { + joint_trajectory_server->setPreempted(); +} + +void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj:: +onFollowJointTrajectoryActionPreempt() { + follow_joint_trajectory_server->setPreempted(); +} + +extern "C" +{ + + void HrpsysJointTrajectoryBridgeInit(RTC::Manager* manager) + { + coil::Properties profile(hrpsysjointtrajectorybridge_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h new file mode 100644 index 000000000..b2db6c9ed --- /dev/null +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h @@ -0,0 +1,111 @@ +// -*- C++ -*- +/*! + * @file HrpsysJointTrajectoryBridge.h * @brief hrpsys setJointAngle - ros joint trajectory bridge * @date $Date$ + * + * $Id$ + */ +#ifndef HRPSYSJOINTTRAJECTORYBRIDGE_H +#define HRPSYSJOINTTRAJECTORYBRIDGE_H + +#include +#include +#include +#include +#include +#include +#include +#include + +// hrp +#include "HRPDataTypes.hh" +#include +#include +#include +#include +#include + +// ros +#include "ros/ros.h" +#include "sensor_msgs/JointState.h" +#include "control_msgs/FollowJointTrajectoryAction.h" +#include "actionlib/server/simple_action_server.h" +#include "pr2_controllers_msgs/JointTrajectoryAction.h" +#include "pr2_controllers_msgs/JointTrajectoryControllerState.h" + +#include "hrpsys/idl/SequencePlayerServiceStub.h" + +using namespace RTC; + +class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase +{ + public: + HrpsysJointTrajectoryBridge(RTC::Manager* manager); + ~HrpsysJointTrajectoryBridge(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + RTC::ReturnCode_t onInitialize(); + RTC::ReturnCode_t onFinalize(); + RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + class jointTrajectoryActionObj + { + protected: + HrpsysJointTrajectoryBridge *parent; + + ros::Publisher joint_controller_state_pub; + + boost::shared_ptr < + actionlib::SimpleActionServer< + pr2_controllers_msgs::JointTrajectoryAction > > joint_trajectory_server; + boost::shared_ptr < + actionlib::SimpleActionServer < + control_msgs::FollowJointTrajectoryAction > > follow_joint_trajectory_server; + + std::string controller_name; + std::string groupname; + std::vector joint_list; + bool interpolationp; + + public: + typedef boost::shared_ptr Ptr; + jointTrajectoryActionObj(HrpsysJointTrajectoryBridge *ptr, + std::string &cname, std::string &gname, std::vector &jlist); + ~jointTrajectoryActionObj(); + + void onJointTrajectory(trajectory_msgs::JointTrajectory trajectory); + void onJointTrajectoryActionGoal(); + void onJointTrajectoryActionPreempt(); + void onFollowJointTrajectoryActionGoal(); + void onFollowJointTrajectoryActionPreempt(); + + void proc(); + }; + + protected: + RTC::CorbaPort m_SequencePlayerServicePort; + RTC::CorbaConsumer m_service0; + + protected: + hrp::BodyPtr body; + OpenHRP::BodyInfo_var bodyinfo; + + ros::NodeHandle nh; + std::vector < jointTrajectoryActionObj::Ptr > trajectory_actions; + coil::TimeMeasure tm; + coil::Mutex m_mutex; + std::string nameserver; + + private: + +}; + + +extern "C" +{ + DLL_EXPORT void HrpsysJointTrajectoryBridgeInit(RTC::Manager* manager); +}; + +#endif // HRPSYSJOINTTRAJECTORYBRIDGE_H + diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridgeComp.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridgeComp.cpp new file mode 100644 index 000000000..a81b7c88e --- /dev/null +++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridgeComp.cpp @@ -0,0 +1,52 @@ +// -*- C++ -*- +/*! + * @file HrpsysJointTrajectoryBridgeComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ +#include +#include +#include +#include "HrpsysJointTrajectoryBridge.h" + +#include + +void MyModuleInit(RTC::Manager* manager) +{ + HrpsysJointTrajectoryBridgeInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("HrpsysJointTrajectoryBridge"); + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + ros::init(argc, argv, "HrpsysJointTrajectoryBridge", 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_ros_bridge/src/README.HrpsysJointTrajectoryBridge b/hrpsys_ros_bridge/src/README.HrpsysJointTrajectoryBridge new file mode 100644 index 000000000..fbd0a87b8 --- /dev/null +++ b/hrpsys_ros_bridge/src/README.HrpsysJointTrajectoryBridge @@ -0,0 +1,182 @@ +#====================================================================== +# RTComponent: HrpsysJointTrajectoryBridge specificatioin +# +# OpenRTM-aist-1.1.0 +# +# Date: Tue Mar 19 18:48:49 2013 +# +# This file is generated by rtc-template with the following argments. +# +# \ +# /home/pr2admin/ros/fuerte/rtm-ros-robotics/rtmros_common/openrtm/bin/rtc-template \ +# -bcxx --module-name=HrpsysJointTrajectoryBridge \ +# --module-type=DataFlowComponent \ +# --module-desc=hrpsys setJointAngle - ros joint trajectory bridge \ +# --module-version=1.0 --module-vendor=Yohei Kakiuchi \ +# --module-category=example --module-comp-type=DataFlowComponent \ +# --module-act-type=SPORADIC --module-max-inst=10 \ +# --consumer=SequencePlayerService:service0:SequencePlayerService \ +# --consumer-idl=/home/pr2admin/ros/fuerte/rtm-ros-robotics/rtmros_common/hrpsys/idl/SequencePlayerService.idl +# +#====================================================================== +# Basic Information +#====================================================================== +# +Basic Information: + Description: hrpsys setJointAngle - ros joint trajectory bridge + Version: 1.0 + Author: Yohei Kakiuchi + Category: example + Comp. Type: DataFlowComponent + Act. Type: SPORADIC + MAX Inst.: 10 + Lang: + Lang Type: + +# +#====================================================================== +# Activity definition +#====================================================================== +# +Actions: + onInitialize: + Description: + onInitialize description + PreCondition: + onInitialize Pre_condition + PostCondition: + onInitialize Post_condition + + onFinalize: + Description: + onFinalize description + PreCondition: + onFinalize Pre_condition + PostCondition: + onFinalize Post_condition + + onActivated: + Description: + onActivated description + PreCondition: + onActivated Pre_condition + PostCondition: + onActivated Post_condition + + onDeactivated: + Description: + onDeactivated description + PreCondition: + onDeactivated Pre_condition + PostCondition: + onDeactivated Post_condition + + onAborting: + Description: + onAborting description + PreCondition: + onAborting Pre_condition + PostCondition: + onAborting Post_condition + + onError: + Description: + onError description + PreCondition: + onError Pre_condition + PostCondition: + onError Post_condition + + onReset: + Description: + onReset description + PreCondition: + onReset Pre_condition + PostCondition: + onReset Post_condition + + onExecute: + Description: + onExecute description + PreCondition: + onExecute Pre_condition + PostCondition: + onExecute Post_condition + + onStateUpdate: + Description: + onStateUpdate description + PreCondition: + onStateUpdate Pre_condition + PostCondition: + onStateUpdate Post_condition + + onShutdown: + Description: + onShutdown description + PreCondition: + onShutdown Pre_condition + PostCondition: + onShutdown Post_condition + + onStartup: + Description: + onStartup description + PreCondition: + onStartup Pre_condition + PostCondition: + onStartup Post_condition + + onRateChanged: + Description: + onRateChanged description + PreCondition: + onRateChanged Pre_condition + PostCondition: + onRateChanged Post_condition + + +# +#====================================================================== +# InPorts definition +#====================================================================== +# +InPorts: + +# +#====================================================================== +# OutPorts definition +#====================================================================== +# +OutPorts: + +# +#====================================================================== +# Service definition +#====================================================================== +# +ServicePorts: + PortName: SequencePlayerService + Description: + InterfaceDescription: + Position: LEFT + Interfaces: + Name: service0 + Description: + Type: SequencePlayerService + Direction: Required + InstanceName: service0 + IDLfile: /home/pr2admin/ros/fuerte/rtm-ros-robotics/rtmros_common/hrpsys/idl/SequencePlayerService.idl + FilePath: + + + +# +#====================================================================== +# Configuration definition +#====================================================================== +# +Configuration: + +# +