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] [hrpsys_ros_bridge] Start joint trajectory at assigned timing #1052

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open

[WIP] [hrpsys_ros_bridge] Start joint trajectory at assigned timing #1052

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ab0f0df
Set time_from_start to feedback of joint trajectory action
pazeshun Jul 9, 2018
c54fe61
Stop feedback when trajectory is completed
pazeshun Jul 10, 2018
f347672
prev_traj_tm -> traj_start_tm
pazeshun Jul 11, 2018
3d7c61e
Start joint trajectory at assigned timing
pazeshun Jul 12, 2018
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
41 changes: 36 additions & 5 deletions hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -307,8 +307,15 @@ HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::~jointTrajectoryActionObj

void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
{
// finish interpolation
ros::Time tm_on_execute = ros::Time::now();

#ifdef USE_PR2_CONTROLLERS_MSGS
// start postponed joint trajectory
if (joint_trajectory_server->isActive() && interpolationp == false && pr2_traj_start_tm <= tm_on_execute)
{
onJointTrajectory(postponed_pr2_traj);
}
// finish interpolation
if (joint_trajectory_server->isActive() && interpolationp == true && parent->m_service0->isEmpty() == true)
{
pr2_controllers_msgs::JointTrajectoryResult result;
Expand All @@ -317,6 +324,12 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @proc joint_trajectory_server->setSucceeded()");
}
#endif
// start postponed joint trajectory
if (follow_joint_trajectory_server->isActive() && interpolationp == false && traj_start_tm <= tm_on_execute)
{
onJointTrajectory(postponed_traj);
}
// finish interpolation
if (follow_joint_trajectory_server->isActive() && interpolationp == true && parent->m_service0->isEmpty() == true)
{
control_msgs::FollowJointTrajectoryResult result;
Expand All @@ -326,10 +339,9 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @proc follow_joint_trajectory_server->setSucceeded()");
}

ros::Time tm_on_execute = ros::Time::now();

// FIXME: need to set actual informatoin, currently we set dummy information
trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point;
commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
commanded_joint_trajectory_point.positions.resize(joint_list.size());
commanded_joint_trajectory_point.velocities.resize(joint_list.size());
commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
Expand All @@ -341,6 +353,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
commanded_joint_trajectory_point.effort[j] = parent->body->link(joint_list[j])->u;
}
error_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
error_joint_trajectory_point.positions.resize(joint_list.size());
error_joint_trajectory_point.velocities.resize(joint_list.size());
error_joint_trajectory_point.accelerations.resize(joint_list.size());
Expand Down Expand Up @@ -511,15 +524,33 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
{
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionGoal");
pr2_controllers_msgs::JointTrajectoryGoalConstPtr goal = joint_trajectory_server->acceptNewGoal();
onJointTrajectory(goal->trajectory);
pr2_traj_start_tm = goal->trajectory.header.stamp;
if (pr2_traj_start_tm <= ros::Time::now())
{
onJointTrajectory(goal->trajectory);
}
else
{
// trajectory is postponed as it is in future
postponed_pr2_traj = goal->trajectory;
}
}
#endif

void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionGoal()
{
ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionGoal()");
control_msgs::FollowJointTrajectoryGoalConstPtr goal = follow_joint_trajectory_server->acceptNewGoal();
onJointTrajectory(goal->trajectory);
traj_start_tm = goal->trajectory.header.stamp;
if (traj_start_tm <= ros::Time::now())
{
onJointTrajectory(goal->trajectory);
}
else
{
// trajectory is postponed as it is in future
postponed_traj = goal->trajectory;
}
}

#ifdef USE_PR2_CONTROLLERS_MSGS
Expand Down
6 changes: 6 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 @@ -102,6 +102,12 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
std::string groupname;
std::vector<std::string> joint_list;
bool interpolationp;
#ifdef USE_PR2_CONTROLLERS_MSGS
trajectory_msgs::JointTrajectory postponed_pr2_traj;
ros::Time pr2_traj_start_tm;
#endif
trajectory_msgs::JointTrajectory postponed_traj;
ros::Time traj_start_tm;

public:
typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
Expand Down
34 changes: 31 additions & 3 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -225,14 +225,26 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
#ifdef USE_PR2_CONTROLLERS_MSGS
void HrpsysSeqStateROSBridge::onJointTrajectoryActionGoal() {
pr2_controllers_msgs::JointTrajectoryGoalConstPtr goal = joint_trajectory_server.acceptNewGoal();
onJointTrajectory(goal->trajectory);
pr2_traj_start_tm = goal->trajectory.header.stamp;
if ( pr2_traj_start_tm <= ros::Time::now() ) {
onJointTrajectory(goal->trajectory);
} else {
// trajectory is postponed as it is in future
postponed_pr2_traj = goal->trajectory;
}
}
#endif

void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
control_msgs::FollowJointTrajectoryGoalConstPtr goal = follow_joint_trajectory_server.acceptNewGoal();
follow_action_initialized = true;
onJointTrajectory(goal->trajectory);
traj_start_tm = goal->trajectory.header.stamp;
if ( traj_start_tm <= ros::Time::now() ) {
onJointTrajectory(goal->trajectory);
} else {
// trajectory is postponed as it is in future
postponed_traj = goal->trajectory;
}
}

#ifdef USE_PR2_CONTROLLERS_MSGS
Expand Down Expand Up @@ -442,6 +454,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
//joint_state.velocity
//joint_state.effort
follow_joint_trajectory_feedback.joint_names.push_back(j->name);
follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - traj_start_tm;
follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - traj_start_tm;
follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - traj_start_tm;
follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
follow_joint_trajectory_feedback.error.positions.push_back(0);
Expand Down Expand Up @@ -486,13 +501,25 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
updateSensorTransform(sensor_tf_stamp); // transform depends on joint angles, not sensor values

#ifdef USE_PR2_CONTROLLERS_MSGS
// start postponed joint trajectory
if ( joint_trajectory_server.isActive() &&
interpolationp == false && pr2_traj_start_tm <= tm_on_execute ) {
onJointTrajectory(postponed_pr2_traj);
}
// finish interpolation
if ( joint_trajectory_server.isActive() &&
interpolationp == true && m_service0->isEmpty() == true ) {
pr2_controllers_msgs::JointTrajectoryResult result;
joint_trajectory_server.setSucceeded(result);
interpolationp = false;
}
#endif
// start postponed joint trajectory
if ( follow_joint_trajectory_server.isActive() &&
interpolationp == false && traj_start_tm <= tm_on_execute ) {
onJointTrajectory(postponed_traj);
}
// finish interpolation
if ( follow_joint_trajectory_server.isActive() &&
interpolationp == true && m_service0->isEmpty() == true ) {
control_msgs::FollowJointTrajectoryResult result;
Expand Down Expand Up @@ -574,7 +601,8 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
}
if ( !follow_joint_trajectory_feedback.joint_names.empty() &&
!follow_joint_trajectory_feedback.actual.positions.empty() &&
follow_action_initialized ) {
follow_action_initialized &&
follow_joint_trajectory_server.isActive() ) {
follow_joint_trajectory_server.publishFeedback(follow_joint_trajectory_feedback);
}
} // end: m_mcangleIn
Expand Down
6 changes: 6 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,12 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl
void clock_cb(const rosgraph_msgs::ClockPtr& str) {};

bool follow_action_initialized;
#ifdef USE_PR2_CONTROLLERS_MSGS
trajectory_msgs::JointTrajectory postponed_pr2_traj;
ros::Time pr2_traj_start_tm;
#endif
trajectory_msgs::JointTrajectory postponed_traj;
ros::Time traj_start_tm;

boost::mutex tf_mutex;
double tf_rate;
Expand Down