Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add rsangle port to /HrpsysJointTrajectoryBridge to publish current j… #995

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add rsangle port to /HrpsysJointTrajectoryBridge to publish current j… #995

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@
output="screen" args ="$(arg openrtm_args)" />

<rtconnect from="$(arg SIMULATOR_NAME_ANGLE).rtc:q" to="HrpsysSeqStateROSBridge0.rtc:rsangle" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
<rtconnect from="$(arg SIMULATOR_NAME_ANGLE).rtc:q" to="HrpsysJointTrajectoryBridge0.rtc:rsangle" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)"/>
<rtconnect from="$(arg SIMULATOR_NAME_VELOCITY).rtc:dq" to="HrpsysSeqStateROSBridge0.rtc:rsvel" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)" if="$(arg USE_VELOCITY_OUTPUT)" />
<rtconnect from="$(arg SIMULATOR_NAME_TORQUE).rtc:tau" to="HrpsysSeqStateROSBridge0.rtc:rstorque" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)" push_rate="$(arg push_rate)" buffer_length="$(arg buffer_length)" unless="$(arg USE_TORQUEFILTER)"/>
<rtconnect from="StateHolderServiceROSBridge.rtc:StateHolderService" to="sh.rtc:StateHolderService" subscription_type="new"/>
Expand Down
33 changes: 32 additions & 1 deletion hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ static const char* hrpsysjointtrajectorybridge_spec[] = {"implementation_id", "H
HrpsysJointTrajectoryBridge::HrpsysJointTrajectoryBridge(RTC::Manager* manager)
// <rtc-template block="initializer">
:
RTC::DataFlowComponentBase(manager), m_SequencePlayerServicePort("SequencePlayerService")
RTC::DataFlowComponentBase(manager), m_rsangleIn("rsangle", m_rsangle), m_SequencePlayerServicePort("SequencePlayerService")
// </rtc-template>
{
}
Expand All @@ -68,6 +68,8 @@ HrpsysJointTrajectoryBridge::~HrpsysJointTrajectoryBridge()

RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onInitialize()
{
addInPort("rsangle", m_rsangleIn);

m_SequencePlayerServicePort.registerConsumer("service0", "SequencePlayerService", m_service0);

addPort(m_SequencePlayerServicePort);
Expand Down Expand Up @@ -202,6 +204,35 @@ RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onFinalize()

RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onExecute(RTC::UniqueId ec_id)
{
// m_in_rsangleIn
if ( m_rsangleIn.isNew () ) {
ROS_DEBUG_STREAM("[" << getInstanceName() << "] @onExecute ec_id : " << ec_id << ", rs:" << m_rsangleIn.isNew());
try {
m_rsangleIn.read();
//for ( unsigned int i = 0; i < m_rsangle.data.length() ; i++ ) std::cerr << m_rsangle.data[i] << " "; std::cerr << std::endl;
}
catch(const std::runtime_error &e)
{
ROS_ERROR_STREAM("[" << getInstanceName() << "] m_rsangleIn failed with " << e.what());
}
m_mutex.lock();
body->calcForwardKinematics();
if ( m_rsangle.data.length() < body->joints().size() ) {
ROS_ERROR_STREAM("rsangle.data.length(" << m_rsangle.data.length() << ") is not equal to body->joints().size(" << body->joints().size() << ")");
m_mutex.unlock();
return RTC::RTC_OK;
} else if ( m_rsangle.data.length() != body->joints().size() ) {
ROS_INFO_STREAM("rsangle.data.length(" << m_rsangle.data.length() << ") is not equal to body->joints().size(" << body->joints().size() << ")");
}
for ( unsigned int i = 0; i < body->joints().size() ; i++ ){
body->joint(i)->q = m_rsangle.data[i];
ROS_DEBUG_STREAM(m_rsangle.data[i] << " ");
}
ROS_DEBUG_STREAM(std::endl);
body->calcForwardKinematics();
m_mutex.unlock();
}
// ros stuff
ros::spinOnce();
for (size_t i = 0; i < trajectory_actions.size(); i++)
{
Expand Down
5 changes: 5 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -115,9 +115,14 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
};

protected:
RTC::TimedDoubleSeq m_rsangle;
RTC::InPort<RTC::TimedDoubleSeq> m_rsangleIn;

RTC::CorbaPort m_SequencePlayerServicePort;
RTC::CorbaConsumer<OpenHRP::SequencePlayerService> m_service0;

std::string hrpsys_version;

protected:
hrp::BodyPtr body;
OpenHRP::BodyInfo_var bodyinfo;
Expand Down