GraspSponge.cpp

Go to the documentation of this file.

/*
 * GraspSponge.cpp
 *
 *  Created on: Sep 27, 2012
 *      Author: tidyup
 */

#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<
                pr2_controllers_msgs::JointTrajectoryAction> TrajClient;

class GraspSponge {
private:
        // Action client for the joint trajectory action
        // used to trigger the arm movement action
        TrajClient* traj_client_;

public:
        GraspSponge();
        ~GraspSponge();

        void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal);
        pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory();

        GraspSponge::GraspSponge() {
                // tell the action client that we want to spin a thread by default
                traj_client_ = new TrajClient(
                                "r_arm_controller/joint_trajectory_action", true);

                // wait for action server to come up
                while (!traj_client_->waitForServer(ros::Duration(5.0))) {
                        ROS_INFO("Waiting for the joint_trajectory_action server");
                }
        }
};
GraspSponge::~GraspSponge() {
        delete traj_client_;
}

void GraspSponge::startTrajectory(
                pr2_controllers_msgs::JointTrajectoryGoal goal) {
        // When to start the trajectory: 1s from now
        goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
        traj_client_->sendGoal(goal);
}


pr2_controllers_msgs::JointTrajectoryGoal GraspSponge::armExtensionTrajectory() {
        //our goal variable
        pr2_controllers_msgs::JointTrajectoryGoal goal;

        // First, the joint names, which apply to all waypoints
        goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
        goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
        goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
        goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
        goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
        goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
        goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

        // We will have two waypoints in this goal trajectory
        goal.trajectory.points.resize(2);

        // First trajectory point
        // Positions
        int ind = 0;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = 0.0;
        goal.trajectory.points[ind].positions[1] = 0.0;
        goal.trajectory.points[ind].positions[2] = 0.0;
        goal.trajectory.points[ind].positions[3] = 0.0;
        goal.trajectory.points[ind].positions[4] = 0.0;
        goal.trajectory.points[ind].positions[5] = 0.0;
        goal.trajectory.points[ind].positions[6] = 0.0;
        // Velocities
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j) {
                goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        // To be reached 1 second after starting along the trajectory
        goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

        // Second trajectory point
        // Positions
        ind += 1;
        goal.trajectory.points[ind].positions.resize(7);
        goal.trajectory.points[ind].positions[0] = -0.3;
        goal.trajectory.points[ind].positions[1] = 0.2;
        goal.trajectory.points[ind].positions[2] = -0.1;
        goal.trajectory.points[ind].positions[3] = -1.2;
        goal.trajectory.points[ind].positions[4] = 1.5;
        goal.trajectory.points[ind].positions[5] = -0.3;
        goal.trajectory.points[ind].positions[6] = 0.5;
        // Velocities
        goal.trajectory.points[ind].velocities.resize(7);
        for (size_t j = 0; j < 7; ++j) {
                goal.trajectory.points[ind].velocities[j] = 0.0;
        }
        // To be reached 2 seconds after starting along the trajectory
        goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);

        //we are done; return the goal
        return goal;
}

actionlib::SimpleClientGoalState GraspSponge::getState() {
        return traj_client_->getState();
}

int main(int argc, char** argv) {
        // Init the ROS node
        ros::init(argc, argv, "robot_driver");

        GraspSponge arm;
        // Start the trajectory
        arm.startTrajectory(arm.armExtensionTrajectory());
        // Wait for trajectory completion
        while (!arm.getState().isDone() && ros::ok()) {
                usleep(50000);
        }
}