diff --git a/.gitignore b/.gitignore index 9154f7f3d..9e6f5e458 100644 --- a/.gitignore +++ b/.gitignore @@ -36,3 +36,6 @@ msg/_.*\.py$ # Catkin custom files CATKIN_IGNORE + +# gedit +*~ diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt index 66848dd00..dac4a819d 100644 --- a/hrpsys_ros_bridge/CMakeLists.txt +++ b/hrpsys_ros_bridge/CMakeLists.txt @@ -105,6 +105,7 @@ string(RANDOM _random_string) 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(RangeSensorROSBridge src/RangeSensorROSBridge.cpp src/RangeSensorROSBridgeComp.cpp) +rtmbuild_add_executable(MasterSlaveROSBridge src/MasterSlaveROSBridge.cpp src/MasterSlaveROSBridgeComp.cpp) rtmbuild_add_executable(PointCloudROSBridge src/PointCloudROSBridge.cpp src/PointCloudROSBridgeComp.cpp) rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 69e25c91f..68c8bad6b 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -32,6 +32,7 @@ <arg name="USE_HRPSYS_PROFILE" default="true" /> <arg name="USE_VELOCITY_OUTPUT" default="false" /> <arg name="PUBLISH_SENSOR_TF" default="true" /> + <arg name="IS_MASTER_SIDE_OF_MASTERSLAVE" default="false" /> <!-- roslaunch hrpsys_ros_bridge sample.launch nameserver:=192.168.1.1 --> @@ -97,7 +98,7 @@ <env name="push_policy" value="$(arg push_policy)" /> <env name="push_rate" value="$(arg push_rate)" /> <env name="buffer_length" value="$(arg buffer_length)" /> - <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT)" output="$(arg OUTPUT)"/> + <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT) IS_MASTER_SIDE_OF_MASTERSLAVE=$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" output="$(arg OUTPUT)"/> <!-- All Robots --> @@ -146,7 +147,7 @@ <rtactivate component="RobotHardwareServiceROSBridge.rtc" /> </group> - <!-- USE_WALING + <!-- USE_WALKING AutoBalancer Stabilizer --> @@ -173,6 +174,76 @@ <rtactivate component="StabilizerServiceROSBridge.rtc" /> <rtactivate component="KalmanFilterServiceROSBridge.rtc" /> </group> + + + <!-- Haptic Coltroller HC - Master Slave WBMS --> + <arg name="openrtm_args_high" default='-o "corba.master_manager:$(arg nameserver):$(arg managerport)" -o "corba.nameservers:$(arg nameserver):$(arg corbaport)" -o "naming.formats:%n.rtc" -o "exec_cxt.periodic.type:PeriodicExecutionContext" -o "exec_cxt.periodic.rate:1000" -o "logger.file_name:/tmp/rtc%p.log"' /> + <node pkg="hrpsys_ros_bridge" name="MasterSlaveROSBridge" type="MasterSlaveROSBridge" args="$(arg openrtm_args_high) -o model:'file://$(arg MODEL_FILE)' -o example.MasterSlaveROSBridge.config_file:$(arg CONF_FILE)" output="$(arg OUTPUT)"> + <param name="is_master_side" type="bool" value="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" /> + </node> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:teleopOdom" to="MasterSlaveROSBridge0.rtc:teleopOdom" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lleg_pose" to="MasterSlaveROSBridge0.rtc:master_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rleg_pose" to="MasterSlaveROSBridge0.rtc:master_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_larm_pose" to="MasterSlaveROSBridge0.rtc:master_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rarm_pose" to="MasterSlaveROSBridge0.rtc:master_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_com_pose" to="MasterSlaveROSBridge0.rtc:master_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_head_pose" to="MasterSlaveROSBridge0.rtc:master_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lhand_pose" to="MasterSlaveROSBridge0.rtc:master_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rhand_pose" to="MasterSlaveROSBridge0.rtc:master_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lfloor_pose" to="MasterSlaveROSBridge0.rtc:master_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rfloor_pose" to="MasterSlaveROSBridge0.rtc:master_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lleg_wrench" to="MasterSlaveROSBridge0.rtc:master_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rleg_wrench" to="MasterSlaveROSBridge0.rtc:master_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_larm_wrench" to="MasterSlaveROSBridge0.rtc:master_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rarm_wrench" to="MasterSlaveROSBridge0.rtc:master_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:delay_check_packet_outbound" to="MasterSlaveROSBridge0.rtc:delay_check_packet_outbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lleg_wrench" to="hc.rtc:slave_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rleg_wrench" to="hc.rtc:slave_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_larm_wrench" to="hc.rtc:slave_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rarm_wrench" to="hc.rtc:slave_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lleg_pose" to="hc.rtc:slave_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rleg_pose" to="hc.rtc:slave_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_larm_pose" to="hc.rtc:slave_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rarm_pose" to="hc.rtc:slave_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_com_pose" to="hc.rtc:slave_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_head_pose" to="hc.rtc:slave_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lhand_pose" to="hc.rtc:slave_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rhand_pose" to="hc.rtc:slave_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lfloor_pose" to="hc.rtc:slave_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rfloor_pose" to="hc.rtc:slave_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:delay_check_packet_inbound" to="hc.rtc:delay_check_packet_inbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lleg_pose" to="wbms.rtc:master_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rleg_pose" to="wbms.rtc:master_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_larm_pose" to="wbms.rtc:master_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rarm_pose" to="wbms.rtc:master_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_com_pose" to="wbms.rtc:master_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_head_pose" to="wbms.rtc:master_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lhand_pose" to="wbms.rtc:master_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rhand_pose" to="wbms.rtc:master_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lfloor_pose" to="wbms.rtc:master_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rfloor_pose" to="wbms.rtc:master_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lleg_wrench" to="wbms.rtc:master_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rleg_wrench" to="wbms.rtc:master_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_larm_wrench" to="wbms.rtc:master_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rarm_wrench" to="wbms.rtc:master_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:delay_check_packet_inbound" to="wbms.rtc:delay_check_packet_inbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lleg_wrench" to="MasterSlaveROSBridge0.rtc:slave_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rleg_wrench" to="MasterSlaveROSBridge0.rtc:slave_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_larm_wrench" to="MasterSlaveROSBridge0.rtc:slave_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rarm_wrench" to="MasterSlaveROSBridge0.rtc:slave_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lleg_pose" to="MasterSlaveROSBridge0.rtc:slave_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rleg_pose" to="MasterSlaveROSBridge0.rtc:slave_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_larm_pose" to="MasterSlaveROSBridge0.rtc:slave_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rarm_pose" to="MasterSlaveROSBridge0.rtc:slave_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_com_pose" to="MasterSlaveROSBridge0.rtc:slave_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_head_pose" to="MasterSlaveROSBridge0.rtc:slave_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lhand_pose" to="MasterSlaveROSBridge0.rtc:slave_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rhand_pose" to="MasterSlaveROSBridge0.rtc:slave_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lfloor_pose" to="MasterSlaveROSBridge0.rtc:slave_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rfloor_pose" to="MasterSlaveROSBridge0.rtc:slave_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:delay_check_packet_outbound" to="MasterSlaveROSBridge0.rtc:delay_check_packet_outbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/> + <rtactivate component="MasterSlaveROSBridge0.rtc" /> + <!-- USE_COLLISIONCHECK CollisionDetector diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp new file mode 100644 index 000000000..4be587daf --- /dev/null +++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp @@ -0,0 +1,361 @@ +#include "MasterSlaveROSBridge.h" + +static const char* masterslaverosbridge_spec[] = + { + "implementation_id", "MasterSlaveROSBridge", + "type_name", "MasterSlaveROSBridge", + "description", "rtm data - ros bridge", + "version", "1.0", + "vendor", "JSK", + "category", "example", + "activity_type", "SPORADIC", + "kind", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + "" + }; + +MasterSlaveROSBridge::MasterSlaveROSBridge(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), + m_delayCheckPacketOutboundIn("delay_check_packet_outbound", m_delayCheckPacketOutbound), + m_delayCheckPacketInboundOut("delay_check_packet_inbound", m_delayCheckPacketInbound), + m_exDataOut("exData", m_exData), + m_exDataIndexOut("exDataIndex", m_exDataIndex) +{} + +MasterSlaveROSBridge::~MasterSlaveROSBridge(){} + +RTC::ReturnCode_t MasterSlaveROSBridge::onInitialize(){ + addInPort("delay_check_packet_outbound", m_delayCheckPacketOutboundIn); + addOutPort("delay_check_packet_inbound", m_delayCheckPacketInboundOut); + addOutPort("exData", m_exDataOut); + addOutPort("exDataIndex", m_exDataIndexOut); + + ee_names.push_back("lleg"); + ee_names.push_back("rleg"); + ee_names.push_back("larm"); + ee_names.push_back("rarm"); + + tgt_names = ee_names; + tgt_names.push_back("com"); + tgt_names.push_back("head"); + tgt_names.push_back("rhand"); + tgt_names.push_back("lhand"); + tgt_names.push_back("rfloor"); + tgt_names.push_back("lfloor"); + + ros::param::param<bool>("~is_master_side", is_master_side); + ros::param::get("~is_master_side", is_master_side); + + if(is_master_side){ + RTC_INFO_STREAM("Set up ports for MASTER side connection (is_master_side = "<<is_master_side<<")"); + for ( int i=0; i<ee_names.size(); i++) { // read ROS data from Network + std::string n = "slave_"+ee_names[i]+"_wrench"; + slaveEEWrenches_sub[ee_names[i]] = nh.subscribe<geometry_msgs::WrenchStamped>(n, 1, + boost::bind(&MasterSlaveROSBridge::onSlaveEEWrenchCB, this, _1, ee_names[i]), ros::VoidConstPtr(), + ros::TransportHints().unreliable().reliable().tcpNoDelay()); + RTC_INFO_STREAM("register ROS Subscriber " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // write RTM data to HC + std::string n = "slave_"+ee_names[i]+"_wrench"; + m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]])); + registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]); + RTC_INFO_STREAM("register RTC OutPort " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // read RTM data from HC + std::string n = "master_"+tgt_names[i]+"_pose"; + m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]])); + registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]); + RTC_INFO_STREAM("register RTC InPort " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // write ROS data to Network + std::string n = "master_"+tgt_names[i]+"_pose"; + masterTgtPoses_pub[tgt_names[i]] = nh.advertise<geometry_msgs::PoseStamped>(n, 1); + RTC_INFO_STREAM("register ROS Publisher " << n ); + } + ////////////////// + for ( int i=0; i<tgt_names.size(); i++) { // read ROS data from Network + std::string n = "slave_"+tgt_names[i]+"_pose"; + slaveTgtPoses_sub[tgt_names[i]] = nh.subscribe<geometry_msgs::PoseStamped>(n, 1, + boost::bind(&MasterSlaveROSBridge::onSlaveTgtPoseCB, this, _1, tgt_names[i]), ros::VoidConstPtr(), + ros::TransportHints().unreliable().reliable().tcpNoDelay()); + RTC_INFO_STREAM("register ROS Subscriber " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // write RTM data to HC + std::string n = "slave_"+tgt_names[i]+"_pose"; + m_slaveTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]])); + registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt_names[i]]); + RTC_INFO_STREAM("register RTC OutPort " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // read RTM data from HC + std::string n = "master_"+ee_names[i]+"_wrench"; + m_masterEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]])); + registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee_names[i]]); + RTC_INFO_STREAM("register RTC InPort " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // write ROS data to Network + std::string n = "master_"+ee_names[i]+"_wrench"; + masterEEWrenches_pub[ee_names[i]] = nh.advertise<geometry_msgs::WrenchStamped>(n, 1); + RTC_INFO_STREAM("register ROS Publisher " << n ); + } + /////////////////// + {// teleop Odom TF + std::string n = "teleopOdom"; + m_teleopOdomIn = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_teleopOdom)); + registerInPort(n.c_str(), *m_teleopOdomIn); + RTC_INFO_STREAM("register RTC InPort " << n ); + } + + }else{ + RTC_INFO_STREAM("Set up ports for SLAVE side connection (is_master_side = "<<is_master_side<<")"); + for ( int i=0; i<tgt_names.size(); i++) { // to read ROS data from Network + std::string n = "master_"+tgt_names[i]+"_pose"; + masterTgtPoses_sub[tgt_names[i]] = nh.subscribe<geometry_msgs::PoseStamped>(n, 1, + boost::bind(&MasterSlaveROSBridge::onMasterTgtPoseCB, this, _1, tgt_names[i]), ros::VoidConstPtr(), + ros::TransportHints().unreliable().reliable().tcpNoDelay()); + RTC_INFO_STREAM("register ROS Subscriber " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // to write RTM data to WBMS + std::string n = "master_"+tgt_names[i]+"_pose"; + m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]])); + registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]); + RTC_INFO_STREAM("register RTC OutPort " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // to read RTM data from WBMS + std::string n = "slave_"+ee_names[i]+"_wrench"; + m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]])); + registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]); + RTC_INFO_STREAM("register RTC InPort " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // to write ROS data to Network + std::string n = "slave_"+ee_names[i]+"_wrench"; + slaveEEWrenches_pub[ee_names[i]] = nh.advertise<geometry_msgs::WrenchStamped>(n, 1); + RTC_INFO_STREAM("register ROS Publisher " << n ); + } + ///////// + for ( int i=0; i<ee_names.size(); i++) { // to read ROS data from Network + std::string n = "master_"+ee_names[i]+"_wrench"; + masterEEWrenches_sub[ee_names[i]] = nh.subscribe<geometry_msgs::WrenchStamped>(n, 1, + boost::bind(&MasterSlaveROSBridge::onMasterEEWrenchCB, this, _1, ee_names[i]), ros::VoidConstPtr(), + ros::TransportHints().unreliable().reliable().tcpNoDelay()); + RTC_INFO_STREAM("register ROS Subscriber " << n ); + } + for ( int i=0; i<ee_names.size(); i++) { // to write RTM data to WBMS + std::string n = "master_"+ee_names[i]+"_wrench"; + m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]])); + registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]); + RTC_INFO_STREAM("register RTC OutPort " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // to read RTM data from WBMS + std::string n = "slave_"+tgt_names[i]+"_pose"; + m_slaveTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]])); + registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt_names[i]]); + RTC_INFO_STREAM("register RTC InPort " << n ); + } + for ( int i=0; i<tgt_names.size(); i++) { // to write ROS data to Network + std::string n = "slave_"+tgt_names[i]+"_pose"; + slaveTgtPoses_pub[tgt_names[i]] = nh.advertise<geometry_msgs::PoseStamped>(n, 1); + RTC_INFO_STREAM("register ROS Publisher " << n ); + } + } + + // both + delay_check_packet_sub = nh.subscribe("delay_check_packet_inbound", 1, &MasterSlaveROSBridge::ondelayCheckPacketCB, this, ros::TransportHints().unreliable().reliable().tcpNoDelay()); + delay_check_packet_pub = nh.advertise<std_msgs::Time>("delay_check_packet_outbound", 1); + + ROS_INFO_STREAM("[MasterSlaveROSBridge] @Initilize name : " << getInstanceName()); + + loop = 0; + tm.tick(); + return RTC::RTC_OK; +} + +void MasterSlaveROSBridge::ondelayCheckPacketCB(const std_msgs::Time::ConstPtr& msg){ + m_delayCheckPacketInbound.sec = msg->data.sec; + m_delayCheckPacketInbound.nsec = msg->data.nsec; + m_delayCheckPacketInboundOut.write(); +} + +void MasterSlaveROSBridge::onMasterEEWrenchCB(const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key){ + m_masterEEWrenches[key].data.length(6); + m_masterEEWrenches[key].data[0] = msg->wrench.force.x; + m_masterEEWrenches[key].data[1] = msg->wrench.force.y; + m_masterEEWrenches[key].data[2] = msg->wrench.force.z; + m_masterEEWrenches[key].data[3] = msg->wrench.torque.x; + m_masterEEWrenches[key].data[4] = msg->wrench.torque.y; + m_masterEEWrenches[key].data[5] = msg->wrench.torque.z; + m_masterEEWrenchesOut[key]->write(); +} + +void MasterSlaveROSBridge::onSlaveEEWrenchCB(const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key){ + m_slaveEEWrenches[key].data.length(6); + m_slaveEEWrenches[key].data[0] = msg->wrench.force.x; + m_slaveEEWrenches[key].data[1] = msg->wrench.force.y; + m_slaveEEWrenches[key].data[2] = msg->wrench.force.z; + m_slaveEEWrenches[key].data[3] = msg->wrench.torque.x; + m_slaveEEWrenches[key].data[4] = msg->wrench.torque.y; + m_slaveEEWrenches[key].data[5] = msg->wrench.torque.z; + m_slaveEEWrenchesOut[key]->write(); +} + +void MasterSlaveROSBridge::onMasterTgtPoseCB(const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key){ + tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w); + if(quat.length() != 0.0){ + tf::Matrix3x3(quat.normalized()).getRPY(m_masterTgtPoses[key].data.orientation.r, m_masterTgtPoses[key].data.orientation.p, m_masterTgtPoses[key].data.orientation.y); + }else{ + m_masterTgtPoses[key].data.orientation = (RTC::Orientation3D){0,0,0}; + } + m_masterTgtPoses[key].data.position = (RTC::Point3D){msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + m_masterTgtPosesOut[key]->write(); +} + +void MasterSlaveROSBridge::onSlaveTgtPoseCB(const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key){ + tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w); + if(quat.length() != 0.0){ + tf::Matrix3x3(quat.normalized()).getRPY(m_slaveTgtPoses[key].data.orientation.r, m_slaveTgtPoses[key].data.orientation.p, m_slaveTgtPoses[key].data.orientation.y); + }else{ + m_slaveTgtPoses[key].data.orientation = (RTC::Orientation3D){0,0,0}; + } + m_slaveTgtPoses[key].data.position = (RTC::Point3D){msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; + m_slaveTgtPosesOut[key]->write(); +} + + +RTC::ReturnCode_t MasterSlaveROSBridge::onExecute(RTC::UniqueId ec_id){ + ros::Time tm_on_execute = ros::Time::now(); + + + if(is_master_side){ //master + for(int i=0; i<tgt_names.size(); i++){ + if(m_masterTgtPosesIn[tgt_names[i]]->isNew()){ + m_masterTgtPosesIn[tgt_names[i]]->read(); + geometry_msgs::PoseStamped tmp; + tmp.header.stamp = tm_on_execute; + tmp.header.frame_id = "teleop_odom"; + tmp.pose.position.x = m_masterTgtPoses[tgt_names[i]].data.position.x; + tmp.pose.position.y = m_masterTgtPoses[tgt_names[i]].data.position.y; + tmp.pose.position.z = m_masterTgtPoses[tgt_names[i]].data.position.z; + tf::Quaternion quat = tf::createQuaternionFromRPY( + m_masterTgtPoses[tgt_names[i]].data.orientation.r, + m_masterTgtPoses[tgt_names[i]].data.orientation.p, + m_masterTgtPoses[tgt_names[i]].data.orientation.y); + tmp.pose.orientation.x = quat.getX(); + tmp.pose.orientation.y = quat.getY(); + tmp.pose.orientation.z = quat.getZ(); + tmp.pose.orientation.w = quat.getW(); + masterTgtPoses_pub[tgt_names[i]].publish(tmp); + } + } + for(int i=0; i<ee_names.size(); i++){ + if(m_masterEEWrenchesIn[ee_names[i]]->isNew()){ + m_masterEEWrenchesIn[ee_names[i]]->read(); + geometry_msgs::WrenchStamped tmp; + tmp.header.stamp = tm_on_execute; + tmp.header.frame_id = "master_"+ee_names[i]+"_aligned_to_world"; + tmp.wrench.force.x = m_masterEEWrenches[ee_names[i]].data[0]; + tmp.wrench.force.y = m_masterEEWrenches[ee_names[i]].data[1]; + tmp.wrench.force.z = m_masterEEWrenches[ee_names[i]].data[2]; + tmp.wrench.torque.x = m_masterEEWrenches[ee_names[i]].data[3]; + tmp.wrench.torque.y = m_masterEEWrenches[ee_names[i]].data[4]; + tmp.wrench.torque.z = m_masterEEWrenches[ee_names[i]].data[5]; + masterEEWrenches_pub[ee_names[i]].publish(tmp); + } + } + if (m_teleopOdomIn->isNew()) { + m_teleopOdomIn->read(); + geometry_msgs::TransformStamped tf_tmp; + tf_tmp.header.stamp = tm_on_execute; + tf_tmp.header.frame_id = "teleop_odom"; + tf_tmp.child_frame_id = "BASE_LINK"; + tf::Quaternion quat = tf::createQuaternionFromRPY(m_teleopOdom.data.orientation.r, m_teleopOdom.data.orientation.p, m_teleopOdom.data.orientation.y); + tf_tmp.transform.translation.x = m_teleopOdom.data.position.x; + tf_tmp.transform.translation.y = m_teleopOdom.data.position.y; + tf_tmp.transform.translation.z = m_teleopOdom.data.position.z; + tf_tmp.transform.rotation.x = quat.getX(); + tf_tmp.transform.rotation.y = quat.getY(); + tf_tmp.transform.rotation.z = quat.getZ(); + tf_tmp.transform.rotation.w = quat.getW(); + if(loop%10==0){ // will be 100Hz of 1000Hz + std::vector<geometry_msgs::TransformStamped> tf_transforms; + tf_transforms.push_back(tf_tmp); + br.sendTransform(tf_transforms); + } + } + + }else{ // slave + for(int i=0; i<ee_names.size(); i++){ + if(m_slaveEEWrenchesIn[ee_names[i]]->isNew()){ + m_slaveEEWrenchesIn[ee_names[i]]->read(); + geometry_msgs::WrenchStamped tmp; + tmp.header.stamp = tm_on_execute; + tmp.header.frame_id = "slave_"+ee_names[i]+"_aligned_to_world"; + tmp.wrench.force.x = m_slaveEEWrenches[ee_names[i]].data[0]; + tmp.wrench.force.y = m_slaveEEWrenches[ee_names[i]].data[1]; + tmp.wrench.force.z = m_slaveEEWrenches[ee_names[i]].data[2]; + tmp.wrench.torque.x = m_slaveEEWrenches[ee_names[i]].data[3]; + tmp.wrench.torque.y = m_slaveEEWrenches[ee_names[i]].data[4]; + tmp.wrench.torque.z = m_slaveEEWrenches[ee_names[i]].data[5]; + slaveEEWrenches_pub[ee_names[i]].publish(tmp); + } + } + for(int i=0; i<tgt_names.size(); i++){ + if(m_slaveTgtPosesIn[tgt_names[i]]->isNew()){ + m_slaveTgtPosesIn[tgt_names[i]]->read(); + geometry_msgs::PoseStamped tmp; + tmp.header.stamp = tm_on_execute; + tmp.header.frame_id = "slave_teleop_odom"; + tmp.pose.position.x = m_slaveTgtPoses[tgt_names[i]].data.position.x; + tmp.pose.position.y = m_slaveTgtPoses[tgt_names[i]].data.position.y; + tmp.pose.position.z = m_slaveTgtPoses[tgt_names[i]].data.position.z; + tf::Quaternion quat = tf::createQuaternionFromRPY( + m_slaveTgtPoses[tgt_names[i]].data.orientation.r, + m_slaveTgtPoses[tgt_names[i]].data.orientation.p, + m_slaveTgtPoses[tgt_names[i]].data.orientation.y); + tmp.pose.orientation.x = quat.getX(); + tmp.pose.orientation.y = quat.getY(); + tmp.pose.orientation.z = quat.getZ(); + tmp.pose.orientation.w = quat.getW(); + slaveTgtPoses_pub[tgt_names[i]].publish(tmp); + } + } + } + + + // both + if(m_delayCheckPacketOutboundIn.isNew()){ + m_delayCheckPacketOutboundIn.read(); + std_msgs::Time tmp; + tmp.data.sec = m_delayCheckPacketOutbound.sec; + tmp.data.nsec = m_delayCheckPacketOutbound.nsec; + delay_check_packet_pub.publish(tmp); + } + + + static int count = 0; + tm.tack(); + if ( tm.interval() > 1 ) { + ROS_INFO_STREAM("[" << getInstanceName() << "] @onExecutece " << ec_id << " is working at " << count << "[Hz]"); + tm.tick(); + count = 0; + } + count ++; +// } else { // m_range +// double interval = 5; +// tm.tack(); +// if ( tm.interval() > interval ) { +// ROS_WARN_STREAM("[" << getInstanceName() << "] @onExecutece " << ec_id << " is not executed last " << interval << "[sec]"); +// tm.tick(); +// } +// } + + loop++; + ros::spinOnce(); + return RTC::RTC_OK; +} + +extern "C"{ + void MasterSlaveROSBridgeInit(RTC::Manager* manager) { + coil::Properties profile(masterslaverosbridge_spec); + manager->registerFactory(profile, RTC::Create<MasterSlaveROSBridge>, RTC::Delete<MasterSlaveROSBridge>); + } +}; diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h new file mode 100644 index 000000000..18b03183d --- /dev/null +++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h @@ -0,0 +1,108 @@ +#ifndef MASTERSLAVEROSBRIDGE_H +#define MASTERSLAVEROSBRIDGE_H + +#include <rtm/idl/BasicDataTypeSkel.h> +#include <rtm/idl/InterfaceDataTypes.hh> +#include <rtm/Manager.h> +#include <rtm/DataFlowComponentBase.h> +#include <rtm/CorbaPort.h> +#include <rtm/DataInPort.h> +#include <rtm/DataOutPort.h> +// ros +#include "ros/ros.h" +#include "std_msgs/Time.h" +#include "geometry_msgs/PoseStamped.h" +#include "geometry_msgs/WrenchStamped.h" +//#include <tf/transform_listener.h> +#include <tf/transform_broadcaster.h> + +using namespace RTC; + +class MasterSlaveROSBridge : public RTC::DataFlowComponentBase +{ + public: + MasterSlaveROSBridge(RTC::Manager* manager); + ~MasterSlaveROSBridge(); + virtual RTC::ReturnCode_t onInitialize(); + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + void updateOdometryTF(const ros::Time &stamp); + void onMasterTgtPoseCB (const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key); + void onSlaveTgtPoseCB (const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key); + void onMasterEEWrenchCB (const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key); + void onSlaveEEWrenchCB (const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key); + void ondelayCheckPacketCB(const std_msgs::Time::ConstPtr& msg); + + protected: + // used in both case + bool is_master_side; + ros::NodeHandle nh; + coil::TimeMeasure tm; + unsigned long loop; + std::vector<std::string> ee_names, tgt_names; + typedef boost::shared_ptr<RTC::InPort <RTC::TimedPose3D> > ITP3_Ptr; + typedef boost::shared_ptr<RTC::InPort <RTC::TimedDoubleSeq> > ITDS_Ptr; + typedef boost::shared_ptr<RTC::OutPort <RTC::TimedPose3D> > OTP3_Ptr; + typedef boost::shared_ptr<RTC::OutPort <RTC::TimedDoubleSeq> > OTDS_Ptr; + std::map<std::string, RTC::TimedPose3D> m_masterTgtPoses; + std::map<std::string, RTC::TimedPose3D> m_slaveTgtPoses; + std::map<std::string, RTC::TimedDoubleSeq> m_masterEEWrenches; + std::map<std::string, RTC::TimedDoubleSeq> m_slaveEEWrenches; + + // master side + std::map<std::string, ITP3_Ptr> m_masterTgtPosesIn; + std::map<std::string, OTDS_Ptr> m_slaveEEWrenchesOut; + std::map<std::string, ros::Publisher> masterTgtPoses_pub; + std::map<std::string, ros::Subscriber> slaveEEWrenches_sub; + // sub usage + std::map<std::string, ITDS_Ptr> m_masterEEWrenchesIn; + std::map<std::string, OTP3_Ptr> m_slaveTgtPosesOut; + std::map<std::string, ros::Publisher> masterEEWrenches_pub; + std::map<std::string, ros::Subscriber> slaveTgtPoses_sub; + RTC::TimedPose3D m_teleopOdom; + ITP3_Ptr m_teleopOdomIn; + tf::TransformBroadcaster br; + + // slave side + std::map<std::string, ITDS_Ptr> m_slaveEEWrenchesIn; + std::map<std::string, OTP3_Ptr> m_masterTgtPosesOut; + std::map<std::string, ros::Publisher> slaveEEWrenches_pub; + std::map<std::string, ros::Subscriber> masterTgtPoses_sub; + // sub usage + std::map<std::string, ITP3_Ptr> m_slaveTgtPosesIn; + std::map<std::string, OTDS_Ptr> m_masterEEWrenchesOut; + std::map<std::string, ros::Publisher> slaveTgtPoses_pub; + std::map<std::string, ros::Subscriber> masterEEWrenches_sub; + + // delay calc + ros::Subscriber delay_check_packet_sub; + ros::Publisher delay_check_packet_pub; + RTC::Time m_delayCheckPacketInbound, m_delayCheckPacketOutbound; + RTC::OutPort<RTC::Time> m_delayCheckPacketInboundOut; + RTC::InPort<RTC::Time> m_delayCheckPacketOutboundIn; + + RTC::TimedDoubleSeq m_exData; + RTC::TimedStringSeq m_exDataIndex; + RTC::OutPort<RTC::TimedDoubleSeq> m_exDataOut; + RTC::OutPort<RTC::TimedStringSeq> m_exDataIndexOut; +}; + + +////// copy +#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl +#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl +#define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl +#define RTC_INFO_STREAM(var) std::cout << "[" << m_profile.instance_name << "] "<< var << std::endl; +#define RTC_WARN_STREAM(var) std::cerr << "\x1b[31m[" << m_profile.instance_name << "] " << var << "\x1b[39m" << std::endl; + +#define eps_eq(a, b, c) (fabs((a)-(b)) <= c) +#define LIMIT_NORM(x,max) (x= ( x<(-max) ? -max : (x>max ? max : x))) +#define LIMIT_MIN(x,min) (x= ( x<min ? min : x )) +#define LIMIT_MAX(x,max) (x= ( x>max ? max : x )) +#define LIMIT_MINMAX(x,min,max) (x= ( x<min ? min : ( x>max ? max : x ))) +#define LIMIT_NORM_V(v,max) if(v.norm()>max){v=v.normalized()*max;} +#define LIMIT_MIN_V(v,minv) (v= v.cwiseMax(minv)) +#define LIMIT_MAX_V(v,maxv) (v= v.cwiseMin(maxv)) +#define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv)) + +extern "C"{ DLL_EXPORT void MasterSlaveROSBridgeInit(RTC::Manager* manager);}; +#endif // MASTERSLAVEROSBRIDGE_H diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp b/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp new file mode 100644 index 000000000..5b410000b --- /dev/null +++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp @@ -0,0 +1,50 @@ +// -*- C++ -*- +/*! + * @file MasterSlaveROSBridgeComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ +#include <rtm/Manager.h> +#include <iostream> +#include <string> +#include "MasterSlaveROSBridge.h" + +void MyModuleInit(RTC::Manager* manager) +{ + MasterSlaveROSBridgeInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("MasterSlaveROSBridge"); + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + ros::init(argc, argv, "MasterSlaveROSBridgeComp", 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_tools/launch/hrpsys.launch b/hrpsys_tools/launch/hrpsys.launch index bda5a38c1..7bac8c9dd 100644 --- a/hrpsys_tools/launch/hrpsys.launch +++ b/hrpsys_tools/launch/hrpsys.launch @@ -76,6 +76,8 @@ -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)" -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)" -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)" +-o "example.WholeBodyMasterSlave.config_file:$(arg CONF_FILE)" +-o "example.HapticController.config_file:$(arg CONF_FILE)" -o "example.RobotHardware.config_file:$(arg RobotHardware_conf)" '/> <arg name="hrpsys_opt_rtc_config_args" default=''/> diff --git a/rtmbuild/cmake/rtmbuild.cmake b/rtmbuild/cmake/rtmbuild.cmake index b6c983974..14e86e2e9 100644 --- a/rtmbuild/cmake/rtmbuild.cmake +++ b/rtmbuild/cmake/rtmbuild.cmake @@ -137,7 +137,8 @@ macro(rtmbuild_init) rosbuild_gensrv() endif() - include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS}) + #include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS}) + include_directories( ${openhrp3_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) # since catkin > 0.7.0, the CPATH is no longer being set by catkin, so rtmbuild manually add them set(_cmake_prefix_path_tmp $ENV{CMAKE_PREFIX_PATH}) string(REPLACE ":" ";" _cmake_prefix_path_tmp ${_cmake_prefix_path_tmp}) @@ -145,7 +146,8 @@ macro(rtmbuild_init) include_directories(${_cmake_prefix_path}/include) endforeach() - link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS}) + #link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS}) + link_directories(${openhrp3_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS}) endmacro(rtmbuild_init) @@ -308,7 +310,8 @@ macro(rtmbuild_add_executable exe) set_target_properties(${exe} PROPERTIES COMPILE_FLAGS "-Wno-deprecated") ## add_dependencies(${exe} RTMBUILD_${PROJECT_NAME}_genbridge ${${_package}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) - target_link_libraries(${exe} ${catkin_LIBRARIES} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} ) + #target_link_libraries(${exe} ${catkin_LIBRARIES} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} ) + target_link_libraries(${exe} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} ${openrtm_aist_LIBRARIES} ${catkin_LIBRARIES} ) endmacro(rtmbuild_add_executable) macro(rtmbuild_add_library lib) @@ -318,6 +321,7 @@ macro(rtmbuild_add_library lib) else() rosbuild_add_library(${ARGV}) endif() - target_link_libraries(${lib} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS}) + #target_link_libraries(${lib} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS}) + target_link_libraries(${lib} ${openhrp3_LIBRARIES} ${openrtm_aist_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS}) endmacro(rtmbuild_add_library)