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

Set time_from_start to feedback of joint trajectory action #1049

Closed
wants to merge 3 commits into from
Closed

Set time_from_start to feedback of joint trajectory action #1049

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
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
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
3 changes: 3 additions & 0 deletions hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -330,6 +330,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()

// 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 +342,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 @@ -502,6 +504,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
parent->m_service0->playPattern(angles, rpy, zmp, duration);
}
}
traj_start_tm = ros::Time::now();

interpolationp = true;
}
Expand Down
1 change: 1 addition & 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,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
std::string groupname;
std::vector<std::string> joint_list;
bool interpolationp;
ros::Time traj_start_tm;

public:
typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
Expand Down
7 changes: 6 additions & 1 deletion hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -218,6 +218,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
OpenHRP::dSequenceSequence rpy, zmp;
m_service0->playPattern(angles, rpy, zmp, duration);
}
traj_start_tm = ros::Time::now();

interpolationp = true;
}
Expand Down Expand Up @@ -442,6 +443,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 @@ -574,7 +578,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
1 change: 1 addition & 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,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl
void clock_cb(const rosgraph_msgs::ClockPtr& str) {};

bool follow_action_initialized;
ros::Time traj_start_tm;

boost::mutex tf_mutex;
double tf_rate;
Expand Down