Skip to content

Commit

Permalink
add bridge for human tracker
Browse files Browse the repository at this point in the history
  • Loading branch information
@ishiguroJSK
ishiguroJSK committed Jun 8, 2016
1 parent bae6a22 commit 053a18e
Show file tree
Hide file tree
Showing 6 changed files with 59 additions and 16 deletions.
3 changes: 3 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,3 +36,6 @@ msg/_.*\.py$

# Catkin custom files
CATKIN_IGNORE

# gedit
*~
2 changes: 2 additions & 0 deletions hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,8 @@
<!-- for human tracker-->
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htzmp" to="abc.rtc:htzmpIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htcom" to="abc.rtc:htcomIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htrfw" to="abc.rtc:htrfwIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htlfw" to="abc.rtc:htlfwIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htrf" to="abc.rtc:htrfIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htlf" to="abc.rtc:htlfIn" subscription_type="new"/>
<rtconnect from="HrpsysSeqStateROSBridge0.rtc:htrh" to="abc.rtc:htrhIn" subscription_type="new"/>
Expand Down
56 changes: 41 additions & 15 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,14 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) :
odom_pub = nh.advertise<nav_msgs::Odometry>("/odom", 1);
imu_pub = nh.advertise<sensor_msgs::Imu>("/imu", 1);
//ishiguro
ht_zmp_sub = nh.subscribe("/human_tracker_zmp_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerZMPCommandCB, this);
ht_com_sub = nh.subscribe("/human_tracker_com_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerCOMCommandCB, this);
ht_rf_sub = nh.subscribe("/human_tracker_rf_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerRFCommandCB, this);
ht_lf_sub = nh.subscribe("/human_tracker_lf_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerLFCommandCB, this);
ht_rh_sub = nh.subscribe("/human_tracker_rh_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerRHCommandCB, this);
ht_lh_sub = nh.subscribe("/human_tracker_lh_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerLHCommandCB, this);
ht_zmp_sub = nh.subscribe("human_tracker_zmp_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerZMPCommandCB, this);
ht_com_sub = nh.subscribe("human_tracker_com_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerCOMCommandCB, this);
ht_rfw_sub = nh.subscribe("human_tracker_rfw_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerRFWCommandCB, this);
ht_lfw_sub = nh.subscribe("human_tracker_lfw_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerLFWCommandCB, this);
ht_rf_sub = nh.subscribe("human_tracker_rf_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerRFCommandCB, this);
ht_lf_sub = nh.subscribe("human_tracker_lf_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerLFCommandCB, this);
ht_rh_sub = nh.subscribe("human_tracker_rh_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerRHCommandCB, this);
ht_lh_sub = nh.subscribe("human_tracker_lh_ref", 1, &HrpsysSeqStateROSBridge::onHumanTrackerLHCommandCB, this);



Expand Down Expand Up @@ -249,6 +251,16 @@ void HrpsysSeqStateROSBridge::onHumanTrackerCOMCommandCB(const geometry_msgs::Po
m_htcom.data.x = msg->point.x; m_htcom.data.y = msg->point.y; m_htcom.data.z = msg->point.z;
m_htcomOut.write(m_htcom);
}
void HrpsysSeqStateROSBridge::onHumanTrackerRFWCommandCB(const geometry_msgs::WrenchStampedConstPtr& msg) {
m_htrfw.data[0] = msg->wrench.force.x; m_htrfw.data[1] = msg->wrench.force.y; m_htrfw.data[2] = msg->wrench.force.z;
m_htrfw.data[3] = msg->wrench.torque.x; m_htrfw.data[4] = msg->wrench.torque.y; m_htrfw.data[5] = msg->wrench.torque.z;
m_htrfwOut.write(m_htrfw);
}
void HrpsysSeqStateROSBridge::onHumanTrackerLFWCommandCB(const geometry_msgs::WrenchStampedConstPtr& msg) {
m_htlfw.data[0] = msg->wrench.force.x; m_htlfw.data[1] = msg->wrench.force.y; m_htlfw.data[2] = msg->wrench.force.z;
m_htlfw.data[3] = msg->wrench.torque.x; m_htlfw.data[4] = msg->wrench.torque.y; m_htlfw.data[5] = msg->wrench.torque.z;
m_htlfwOut.write(m_htlfw);
}
void HrpsysSeqStateROSBridge::onHumanTrackerRFCommandCB(const geometry_msgs::PointStampedConstPtr& msg) {
m_htrf.data.x = msg->point.x; m_htrf.data.y = msg->point.y; m_htrf.data.z = msg->point.z;
m_htrfOut.write(m_htrf);
Expand Down Expand Up @@ -325,15 +337,29 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
count_all ++;

//ishiguro tf
tf::StampedTransform transform;
try{
ht_tf_listener.lookupTransform("/torso_w","/camera_link",ros::Time(0), transform);
ROS_INFO("[TF] %f, %f, %f\n",transform.getOrigin()[0],transform.getOrigin()[1],transform.getOrigin().z());
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
std::cout<<"[TF MISS]"<<std::endl;
}
// tf::StampedTransform transform;
// try{
// ht_tf_listener.lookupTransform("/torso_1","/world",ros::Time(0), transform);
// ROS_INFO("[TF] %f, %f, %f\n",transform.getOrigin()[0],transform.getOrigin()[1],transform.getOrigin().z());
// m_htcom.data.x = transform.getOrigin()[0]; m_htcom.data.y = transform.getOrigin()[1]; m_htcom.data.z = transform.getOrigin()[2];
// m_htcomOut.write(m_htcom);
// ht_tf_listener.lookupTransform("/right_foot_1","/world",ros::Time(0), transform);
// m_htrf.data.x = transform.getOrigin()[0]; m_htrf.data.y = transform.getOrigin()[1]; m_htrf.data.z = transform.getOrigin()[2];
// m_htrfOut.write(m_htrf);
// ht_tf_listener.lookupTransform("/left_foot_1","/world",ros::Time(0), transform);
// m_htlf.data.x = transform.getOrigin()[0]; m_htlf.data.y = transform.getOrigin()[1]; m_htlf.data.z = transform.getOrigin()[2];
// m_htlfOut.write(m_htlf);
// ht_tf_listener.lookupTransform("/right_hand_1","/world",ros::Time(0), transform);
// m_htrh.data.x = transform.getOrigin()[0]; m_htrh.data.y = transform.getOrigin()[1]; m_htrh.data.z = transform.getOrigin()[2];
// m_htrhOut.write(m_htrh);
// ht_tf_listener.lookupTransform("/left_hand_1","/world",ros::Time(0), transform);
// m_htlh.data.x = transform.getOrigin()[0]; m_htlh.data.y = transform.getOrigin()[1]; m_htlh.data.z = transform.getOrigin()[2];
// m_htlhOut.write(m_htlh);
// }
// catch (tf::TransformException &ex) {
// ROS_ERROR("%s",ex.what());
// std::cout<<"[TF MISS]"<<std::endl;
// }


// servoStateIn
Expand Down
4 changes: 3 additions & 1 deletion hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl
//for human tracker
void onHumanTrackerZMPCommandCB(const geometry_msgs::PointStampedConstPtr& msg);
void onHumanTrackerCOMCommandCB(const geometry_msgs::PointStampedConstPtr& msg);
void onHumanTrackerRFWCommandCB(const geometry_msgs::WrenchStampedConstPtr& msg);
void onHumanTrackerLFWCommandCB(const geometry_msgs::WrenchStampedConstPtr& msg);
void onHumanTrackerRFCommandCB(const geometry_msgs::PointStampedConstPtr& msg);
void onHumanTrackerLFCommandCB(const geometry_msgs::PointStampedConstPtr& msg);
void onHumanTrackerRHCommandCB(const geometry_msgs::PointStampedConstPtr& msg);
Expand Down Expand Up @@ -82,7 +84,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl

ros::Subscriber clock_sub;
//ishiguro
ros::Subscriber ht_zmp_sub,ht_com_sub,ht_rf_sub,ht_lf_sub,ht_rh_sub,ht_lh_sub;
ros::Subscriber ht_zmp_sub,ht_com_sub,ht_rfw_sub,ht_lfw_sub,ht_rf_sub,ht_lf_sub,ht_rh_sub,ht_lh_sub;
tf::TransformListener ht_tf_listener;

nav_msgs::Odometry prev_odom;
Expand Down
6 changes: 6 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager)
//for human tracker
m_htzmpOut("htzmp", m_htzmp),
m_htcomOut("htcom", m_htcom),
m_htrfwOut("htrfw", m_htrfw),
m_htlfwOut("htlfw", m_htlfw),
m_htrfOut("htrf", m_htrf),
m_htlfOut("htlf", m_htlf),
m_htrhOut("htrh", m_htrh),
Expand Down Expand Up @@ -90,10 +92,14 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize()
//for human tracker
addOutPort("htzmp", m_htzmpOut);
addOutPort("htcom", m_htcomOut);
addOutPort("htrfw", m_htrfwOut);
addOutPort("htlfw", m_htlfwOut);
addOutPort("htrf", m_htrfOut);
addOutPort("htlf", m_htlfOut);
addOutPort("htrh", m_htrhOut);
addOutPort("htlh", m_htlhOut);
m_htrfw.data.length(6);
m_htlfw.data.length(6);

// Set service provider to Ports

Expand Down
4 changes: 4 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,6 +160,10 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase
OutPort<TimedPoint3D> m_htzmpOut;
TimedPoint3D m_htcom;
OutPort<TimedPoint3D> m_htcomOut;
TimedDoubleSeq m_htrfw;
OutPort<TimedDoubleSeq> m_htrfwOut;
TimedDoubleSeq m_htlfw;
OutPort<TimedDoubleSeq> m_htlfwOut;
TimedPoint3D m_htrf;
OutPort<TimedPoint3D> m_htrfOut;
TimedPoint3D m_htlf;
Expand Down

0 comments on commit 053a18e

Please sign in to comment.