add joint_trajectory_controller from ros-controls/ros_controllers#411

gundam_rx78_control/CMakeLists.txt

@@ -1,12 +1,27 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(gundam_rx78_control)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS pluginlib)
 
 ###################################
 ## catkin specific configuration ##
 ###################################
-catkin_package()
+catkin_package(
+ CATKIN_DEPENDS pluginlib
+ INCLUDE_DIRS include
+ LIBRARIES ${PROJECT_NAME}
+ )
+
+###########
+## Build ##
+###########
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_library(${PROJECT_NAME}
+ src/joint_trajectory_controller.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 #############
 ## Install ##
@@ -16,6 +31,16 @@ install(DIRECTORY config launch sample
  USE_SOURCE_PERMISSIONS
  )
 
+install(TARGETS ${PROJECT_NAME}
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+ )
+
+
+install(FILES joint_trajectory_controller.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
 #############
 ## Testing ##
 #############

gundam_rx78_control/include/gundam_rx78_control/joint_trajectory_controller.h

@@ -0,0 +1,266 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
+#define JOINT_TRAJECTORY_CONTROLLER_JOINT_TRAJECTORY_CONTROLLER_H
+
+// C++ standard
+#include <cassert>
+#include <iterator>
+#include <stdexcept>
+#include <string>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
+#include <boost/dynamic_bitset.hpp>
+
+// ROS
+#include <ros/node_handle.h>
+
+// URDF
+#include <urdf/model.h>
+
+// ROS messages
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <control_msgs/JointTrajectoryControllerState.h>
+#include <control_msgs/QueryTrajectoryState.h>
+#include <trajectory_msgs/JointTrajectory.h>
+
+// actionlib
+#include <actionlib/server/action_server.h>
+
+// realtime_tools
+#include <realtime_tools/realtime_box.h>
+#include <realtime_tools/realtime_buffer.h>
+#include <realtime_tools/realtime_publisher.h>
+
+// ros_controls
+#include <realtime_tools/realtime_server_goal_handle.h>
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/internal/demangle_symbol.h>
+
+// Project
+#include <trajectory_interface/trajectory_interface.h>
+
+#include <joint_trajectory_controller/joint_trajectory_segment.h>
+#include <joint_trajectory_controller/init_joint_trajectory.h>
+#include <joint_trajectory_controller/hardware_interface_adapter.h>
+
+namespace joint_trajectory_controller
+{
+
+/**
+ * \brief Controller for executing joint-space trajectories on a group of joints.
+ *
+ * \note Non-developer documentation and usage instructions can be found in the package's ROS wiki page.
+ *
+ * \tparam SegmentImpl Trajectory segment representation to use. The type must comply with the following structure:
+ * \code
+ * class FooSegment
+ * {
+ * public:
+ * // Required types
+ * typedef double Scalar; // Scalar can be anything convertible to double
+ * typedef Scalar Time;
+ * typedef PosVelAccState<Scalar> State;
+ *
+ * // Default constructor
+ * FooSegment();
+ *
+ * // Constructor from start and end states (boundary conditions)
+ * FooSegment(const Time& start_time,
+ * const State& start_state,
+ * const Time& end_time,
+ * const State& end_state);
+ *
+ * // Start and end states initializer (the guts of the above constructor)
+ * // May throw std::invalid_argument if parameters are invalid
+ * void init(const Time& start_time,
+ * const State& start_state,
+ * const Time& end_time,
+ * const State& end_state);
+ *
+ * // Sampler (realtime-safe)
+ * void sample(const Time& time, State& state) const;
+ *
+ * // Accesors (realtime-safe)
+ * Time startTime() const;
+ * Time endTime() const;
+ * unsigned int size() const;
+ * };
+ * \endcode
+ *
+ * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface,
+ * \p hardware_interface::VelocityJointInterface, and \p hardware_interface::EffortJointInterface are supported 
+ * out-of-the-box.
+ */
+template <class SegmentImpl, class HardwareInterface>
+class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>
+{
+public:
+
+ JointTrajectoryController();
+
+ /** \name Non Real-Time Safe Functions
+ *\{*/
+ bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+ /*\}*/
+
+ /** \name Real-Time Safe Functions
+ *\{*/
+ /** \brief Holds the current position. */
+ void starting(const ros::Time& time);
+
+ /** \brief Cancels the active action goal, if any. */
+ void stopping(const ros::Time& /*time*/);
+
+ void update(const ros::Time& time, const ros::Duration& period);
+ /*\}*/
+
+protected:
+
+ struct TimeData
+ {
+ TimeData() : time(0.0), period(0.0), uptime(0.0) {}
+
+ ros::Time time; ///< Time of last update cycle
+ ros::Duration period; ///< Period of last update cycle
+ ros::Time uptime; ///< Controller uptime. Set to zero at every restart.
+ };
+
+ typedef actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> ActionServer;
+ typedef boost::shared_ptr<ActionServer> ActionServerPtr;
+ typedef ActionServer::GoalHandle GoalHandle;
+ typedef realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction> RealtimeGoalHandle;
+ typedef boost::shared_ptr<RealtimeGoalHandle> RealtimeGoalHandlePtr;
+ typedef trajectory_msgs::JointTrajectory::ConstPtr JointTrajectoryConstPtr;
+ typedef realtime_tools::RealtimePublisher<control_msgs::JointTrajectoryControllerState> StatePublisher;
+ typedef boost::scoped_ptr<StatePublisher> StatePublisherPtr;
+
+ typedef JointTrajectorySegment<SegmentImpl> Segment;
+ 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;
+
+ typedef HardwareInterfaceAdapter<HardwareInterface, typename Segment::State> HwIfaceAdapter;
+ typedef typename HardwareInterface::ResourceHandleType JointHandle;
+
+ bool verbose_; ///< Hard coded verbose flag to help in debugging
+ std::string name_; ///< Controller name.
+ std::vector<JointHandle> joints_; ///< Handles to controlled joints.
+ std::vector<bool> angle_wraparound_; ///< Whether controlled joints wrap around or not.
+ std::vector<std::string> joint_names_; ///< Controlled joint names.
+ SegmentTolerances<Scalar> default_tolerances_; ///< Default trajectory segment tolerances.
+ HwIfaceAdapter hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface.
+
+ RealtimeGoalHandlePtr rt_active_goal_; ///< Currently active action goal, if any.
+
+ /**
+ * Thread-safe container with a smart pointer to trajectory currently being followed.
+ * Can be either a hold trajectory or a trajectory received from a ROS message.
+ *
+ * We store the hold trajectory in a separate class member because the \p starting(time) method must be realtime-safe.
+ * The (single segment) hold trajectory is preallocated at initialization time and its size is kept unchanged.
+ */
+ 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 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_; ///< Duration for stop ramp. If zero, the controller stops at the actual position.
+ boost::dynamic_bitset<> successful_joint_traj_;
+ bool allow_partial_joints_goal_;
+
+ // ROS API
+ ros::NodeHandle controller_nh_;
+ ros::Subscriber trajectory_command_sub_;
+ ActionServerPtr action_server_;
+ ros::ServiceServer query_state_service_;
+ StatePublisherPtr state_publisher_;
+
+ ros::Timer goal_handle_timer_;
+ ros::Time last_state_publish_time_;
+
+ // Mimic Joints
+ std::vector<JointHandle> mimic_joints_; ///< Handles to mimic joints.
+ std::vector<std::string> mimic_joint_names_; ///< Mimic joint names.
+ std::vector<urdf::JointConstSharedPtr> mimic_urdf_joints_; ///< URDF joint of mimic joints
+ std::vector<unsigned int> mimic_joint_ids_; ///< index of target joint in joints_ vector
+
+ typename Segment::State mimic_current_state_; ///< Preallocated workspace variable.
+ typename Segment::State mimic_desired_state_; ///< Preallocated workspace variable.
+ typename Segment::State mimic_state_error_; ///< Preallocated workspace variable.
+ HwIfaceAdapter mimic_hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface.
+
+ virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh, std::string* error_string = 0);
+ virtual void trajectoryCommandCB(const JointTrajectoryConstPtr& msg);
+ virtual void goalCB(GoalHandle gh);
+ virtual void cancelCB(GoalHandle gh);
+ virtual void preemptActiveGoal();
+ virtual bool queryStateService(control_msgs::QueryTrajectoryState::Request& req,
+ control_msgs::QueryTrajectoryState::Response& resp);
+
+ /**
+ * \brief Publish current controller state at a throttled frequency.
+ * \note This method is realtime-safe and is meant to be called from \ref update, as it shares data with it without
+ * any locking.
+ */
+ void publishState(const ros::Time& time);
+
+ /**
+ * \brief Hold the current position.
+ *
+ * Substitutes the current trajectory with a single-segment one going from the current position and velocity to
+ * zero velocity.
+ * \see parameter stop_trajectory_duration
+ * \note This method is realtime-safe.
+ */
+ void setHoldPosition(const ros::Time& time, RealtimeGoalHandlePtr gh=RealtimeGoalHandlePtr());
+
+};
+
+} // namespace
+
+#include "gundam_rx78_control/joint_trajectory_controller_impl.h"
+#endif // header guard