joint_trajectory_action_controller.cpp

Go to the documentation of this file.

/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "sr_mechanism_controllers/joint_trajectory_action_controller.hpp"
#include <std_msgs/Float64.h>
#include <boost/algorithm/string.hpp>
#include <string>
#include <vector>

namespace shadowrobot
{
  JointTrajectoryActionController::JointTrajectoryActionController()
  {
    action_server = boost::shared_ptr<JTAS>(
            new JTAS("/r_arm_controller/joint_trajectory_action",
                     boost::bind(&JointTrajectoryActionController::execute_trajectory, this, _1),
                     false));

    // Create a map of joint names to their command publishers
    // Hand joints
    // @todo this could be read from the controller manager
    // rosservice call /controller_manager/list_controllers
    std::string hand_names[] =
    {
            "ffj0", "ffj3", "ffj4",
            "lfj0", "lfj3", "lfj4", "lfj5",
            "mfj0", "mfj3", "mfj4",
            "rfj0", "rfj3", "rfj4",
            "thj1", "thj2", "thj3", "thj4", "thj5",
            "wrj1", "wrj2"
    };
    for (size_t i = 0; i < 20; i++)
    {
      joint_pub[hand_names[i]] = nh.advertise<std_msgs::Float64>(
              "/sh_" + hand_names[i] + "_mixed_position_velocity_controller/command", 2);

      joint_pub[boost::to_upper_copy(hand_names[i])] = nh.advertise<std_msgs::Float64>(
              "/sh_" + hand_names[i] + "_mixed_position_velocity_controller/command", 2);
    }

    // Arm joints
    std::string arm_names[] = {"sr", "ss", "es", "er"};
    for (size_t i = 0; i < 4; i++)
    {
      joint_pub[arm_names[i]] = nh.advertise<std_msgs::Float64>(
              "/sa_" + arm_names[i] + "_position_controller/command", 2);
    }

    // Arm joints: 2 naming conventions
    std::string arm_names_2[] = {"ShoulderJRotate", "ShoulderJSwing", "ElbowJSwing", "ElbowJRotate"};
    for (size_t i = 0; i < 4; i++)
    {
      joint_pub[arm_names_2[i]] = nh.advertise<std_msgs::Float64>(
              "/sa_" + arm_names[i] + "_position_controller/command", 2);
    }

    ROS_DEBUG("Starting JointTrajectoryActionController server");
    action_server->start();
  }

  void JointTrajectoryActionController::execute_trajectory(
          const control_msgs::FollowJointTrajectoryGoalConstPtr &goal
  )
  {
    bool success = true;
    std::vector<std::string> joint_names = goal->trajectory.joint_names;
    JointTrajectoryPointVec trajectory_points = goal->trajectory.points;
    trajectory_msgs::JointTrajectoryPoint trajectory_step;

    // @todo We should probably be looking at goal->trajectory.header.stamp to
    // work out what time to start the action.
    // std::cout << goal->trajectory.header.stamp << " - " << ros::Time::now() << std::endl;

    // loop through the steps
    ros::Duration sleeping_time(0.0), last_time(0.0);
    for (size_t index_step = 0; index_step < trajectory_points.size(); ++index_step)
    {
      trajectory_step = trajectory_points[index_step];

      // check if preempted (cancelled), bail out if we are
      if (action_server->isPreemptRequested() || !ros::ok())
      {
        ROS_INFO("Joint Trajectory Action Preempted");
        action_server->setPreempted();
        success = false;
        break;
      }

      // send out the positions for this step to the joints
      for (size_t i = 0; i < joint_names.size(); i++)
      {
        ROS_DEBUG_STREAM("trajectory: " << joint_names[i] << " " << trajectory_step.positions[i] << " / sleep: " <<
                         sleeping_time.toSec());
        ros::Publisher pub = joint_pub[joint_names[i]];
        std_msgs::Float64 msg;
        msg.data = trajectory_step.positions[i];
        pub.publish(msg);
      }
      // Wait until this step is supposed to be completed.
      // @todo This assumes that the movement will be instant! We should really
      // be working out how long the movement will take?
      sleeping_time = trajectory_step.time_from_start - last_time;
      sleeping_time.sleep();
      last_time = trajectory_step.time_from_start;
    }

    // send the result back
    control_msgs::FollowJointTrajectoryResult joint_trajectory_result;
    if (success)
    {
      action_server->setSucceeded(joint_trajectory_result);
    }
    else
    {
      action_server->setAborted(joint_trajectory_result);
    }
  }
}  // namespace shadowrobot


int main(int argc, char **argv)
{
  ros::init(argc, argv, "sr_joint_trajectory_action_controller");

  ros::AsyncSpinner spinner(1);  // Use 1 thread
  spinner.start();
  shadowrobot::JointTrajectoryActionController jac;
  ros::spin();
  // ros::waitForShutdown();

  return 0;
}


/* For the emacs weenies in the crowd.
Local Variables:
   c-basic-offset: 2
End:
*/
// vim: sw=2:ts=2