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

Jump to bottom

[WIP] Tell getting new goal from cancel #10

Closed
wants to merge 3 commits into from
Closed

[WIP] Tell getting new goal from cancel #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a4853ea
Tell getting new goal from cancel
pazeshun Jun 18, 2018
2913445
Add comments to clarify when stopping motion is executed
pazeshun Jun 18, 2018
8881caa
Fix swapping of clear and clearJointAngles
pazeshun Oct 25, 2019
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
86 changes: 47 additions & 39 deletions hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -547,28 +547,32 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
{
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
joint_trajectory_server->setPreempted();
if (groupname.length() > 0)
{ // group
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
// hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
if (!joint_trajectory_server->isNewGoalAvailable())
{ // Cancel request comes from client, so motion should be stopped immediately,
// while motion should be changed smoothly when new goal comes
if (groupname.length() > 0)
{ // group
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
// hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
}
}
}
else
{ // fullbody
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
parent->m_service0->clear();
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
parent->m_service0->clearJointAngles();
else
{ // fullbody
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
parent->m_service0->clear();
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
parent->m_service0->clearJointAngles();
}
}
}
}
Expand All @@ -579,25 +583,29 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
follow_joint_trajectory_server->setPreempted();

if (groupname.length() > 0)
{ // group
// hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
if (!follow_joint_trajectory_server->isNewGoalAvailable())
{ // Cancel request comes from client, so motion should be stopped immediately,
// while motion should be changed smoothly when new goal comes
if (groupname.length() > 0)
{ // group
// hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
}
}
}
else
{ // fullbody
if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
parent->m_service0->clearJointAngles();
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
parent->m_service0->clear();
else
{ // fullbody
if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
parent->m_service0->clear();
} else {
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
parent->m_service0->clearJointAngles();
}
}
}
}
Expand Down
28 changes: 18 additions & 10 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -243,22 +243,30 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
#ifdef USE_PR2_CONTROLLERS_MSGS
void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
joint_trajectory_server.setPreempted();
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
m_service0->clear();
} else {
m_service0->clearJointAngles();
if (!joint_trajectory_server.isNewGoalAvailable()) {
// Cancel request comes from client, so motion should be stopped immediately,
// while motion should be changed smoothly when new goal comes.
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
m_service0->clear();
} else {
m_service0->clearJointAngles();
}
}
}
#endif

void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionPreempt() {
follow_joint_trajectory_server.setPreempted();
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
m_service0->clear();
} else {
m_service0->clearJointAngles();
if (!follow_joint_trajectory_server.isNewGoalAvailable()) {
// Cancel request comes from client, so motion should be stopped immediately,
// while motion should be changed smoothly when new goal comes.
// hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
m_service0->clear();
} else {
m_service0->clearJointAngles();
}
}
}

Expand Down