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

jtc: Enable sending trajectories with a partial set of joints #226

Merged
merged 7 commits into from
Oct 31, 2016
Merged

jtc: Enable sending trajectories with a partial set of joints #226

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
9a4c51d
jtc: Enable sending trajectories with a partial set of joints
May 26, 2016
16361b9
Infrastructure for testing the velocity_controllers::JointTrajectoryC…
miguelprada Jun 17, 2016
0a5694a
Merge branch 'kinetic-devel' into F_enable_part_traj_kinetic
Sep 12, 2016
426d1a9
Merge pull request #228 from miguelprada/velocity_iface_tests
bmagyar Oct 26, 2016
8ebf99b
jtc: Enable sending trajectories with a partial set of joints
May 26, 2016
ecae6be
Merge branch 'F_enable_part_traj_kinetic' of https://github.com/shado…
Oct 28, 2016
c2a903e
Updated copyright info
Oct 31, 2016
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
5 changes: 5 additions & 0 deletions joint_trajectory_controller/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,11 @@ if(CATKIN_ENABLE_TESTING)
test/joint_trajectory_controller.test
test/joint_trajectory_controller_test.cpp)
target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES})

add_rostest_gtest(joint_trajectory_controller_vel_test
test/joint_trajectory_controller_vel.test
test/joint_trajectory_controller_test.cpp)
target_link_libraries(joint_trajectory_controller_vel_test ${catkin_LIBRARIES})
endif()

# Install
Expand Down
280 changes: 189 additions & 91 deletions joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h
View file
Open in desktop

Large diffs are not rendered by default.

44 changes: 11 additions & 33 deletions ...t_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,7 @@
// Boost
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/dynamic_bitset.hpp>

// ROS
#include <ros/node_handle.h>
Expand Down Expand Up @@ -166,8 +167,10 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;

typedef JointTrajectorySegment<SegmentImpl> Segment;
typedef std::vector<Segment> Trajectory;
typedef std::vector<Segment> TrajectoryPerJoint;
typedef std::vector<TrajectoryPerJoint> Trajectory;
typedef boost::shared_ptr<Trajectory> TrajectoryPtr;
typedef boost::shared_ptr<TrajectoryPerJoint> TrajectoryPerJointPtr;
typedef realtime_tools::RealtimeBox<TrajectoryPtr> TrajectoryBox;
typedef typename Segment::Scalar Scalar;

Expand All @@ -194,18 +197,20 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
TrajectoryBox curr_trajectory_box_;
TrajectoryPtr hold_trajectory_ptr_; ///< Last hold trajectory values.

typename Segment::State current_state_; ///< Preallocated workspace variable.
typename Segment::State desired_state_; ///< Preallocated workspace variable.
typename Segment::State state_error_; ///< Preallocated workspace variable.
typename Segment::State hold_start_state_; ///< Preallocated workspace variable.
typename Segment::State hold_end_state_; ///< Preallocated workspace variable.
typename Segment::State current_state_; ///< Preallocated workspace variable.
typename Segment::State desired_state_; ///< Preallocated workspace variable.
typename Segment::State state_error_; ///< Preallocated workspace variable.
typename Segment::State desired_joint_state_; ///< Preallocated workspace variable.
typename Segment::State state_joint_error_; ///< Preallocated workspace variable.

realtime_tools::RealtimeBuffer<TimeData> time_data_;

ros::Duration state_publisher_period_;
ros::Duration action_monitor_period_;

typename Segment::Time stop_trajectory_duration_;
boost::dynamic_bitset<> successful_joint_traj_;
bool allow_partial_joints_goal_;

// ROS API
ros::NodeHandle controller_nh_;
Expand Down Expand Up @@ -241,33 +246,6 @@ class JointTrajectoryController : public controller_interface::Controller<Hardwa
*/
void setHoldPosition(const ros::Time& time);

/**
* \brief Check path tolerances.
*
* If path tolerances are violated, the currently active action goal will be aborted.
*
* \param state_error Error between the current and desired trajectory states.
* \param segment Currently active trajectory segment.
*
* \pre \p segment is associated to the active goal handle.
**/
void checkPathTolerances(const typename Segment::State& state_error,
const Segment& segment);

/**
* \brief Check goal tolerances.
*
* If goal tolerances are fulfilled, the currently active action goal will be considered successful.
* If they are violated, the action goal will be aborted.
*
* \param state_error Error between the current and desired trajectory states.
* \param segment Currently active trajectory segment.
*
* \pre \p segment is associated to the active goal handle.
**/
void checkGoalTolerances(const typename Segment::State& state_error,
const Segment& segment);

};

} // namespace
Expand Down
Loading