ClopemaMove class description

ClopemaMove class contains useful and often called functions. The reason why we created this wrapper was to create more "user friendly" tutorials. The most of tutorials here needs only to plan trajectory for specified goal, execute it and then turn servo power off. This class is placed in library clopema_move in clopema_motoros package so it is not necessary to make local copy. How to link this library with your project is shown in next tutorial.

Header file

/*
 * ClopemaMove.h
 * Wrapper for useful functions used in clopema_planning_tutorials package
 *  Created on: Aug 10, 2012
 *      Author: Vladimír Petrík
 */

#ifndef CLOPEMAMOVE_H_
#define CLOPEMAMOVE_H_

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <clopema_motoros/SetPowerOff.h>
#include <arm_navigation_msgs/GetRobotState.h>
#include <clopema_arm_navigation/ClopemaMotionPlan.h>
#include <arm_navigation_msgs/convert_messages.h>
#include <planning_environment/models/collision_models.h>

class ClopemaMove {
    typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ClopemaMoveClient;

public:
    ClopemaMove();
    virtual ~ClopemaMove();

    /*
     * Send goal to the server and wait for result for maximum 30 sec.
     */
    void doGoal(const control_msgs::FollowJointTrajectoryGoal & goal);
    /*
     * This method is called only once, when goal is done.
     * The process of execution:
     *     1. Print status of the goal (result)
     *     2. Turn servo power off
     */
    void doneCb(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr & result);

    /*
     * Function calls service to turn power off
     * \param force - turn servo power immediately
     * \return false if error occurred
     */
    bool setServoPowerOff(bool force);
    /*
     * Function set robot state (param) to be equal with actual robot position
     * \param RobotState - output robot state
     * \return false if error occurred
     */
    bool getRobotState(arm_navigation_msgs::RobotState & rs);
    /*
     * This function will plan and filter trajectory for request specified in mp param
     * \return false if plan can't be created
     */
    bool plan(clopema_arm_navigation::ClopemaMotionPlan & mp);

    /*
     * Return all joints in group specified in first argument
     * Allowed group: r1_arm, r2_arm, arms, ext_axis
     */
    std::vector<std::string> getJointsInGroup(std::string group);

    /*
     * Convert pose to the motion plan as a goal constraints
     */
    void poseToClopemaMotionPlan(clopema_arm_navigation::ClopemaMotionPlan & mp, arm_navigation_msgs::SimplePoseConstraint & pose);

    /*
     * Convert joint trajectory point to robot state
     */
    void pointToRobotState(arm_navigation_msgs::RobotState &state, trajectory_msgs::JointTrajectoryPoint &point, std::vector<std::string> & joint_names);

    ClopemaMoveClient cmc_;
    const std::string getServerName() {
        return "/clopema_controller/follow_joint_trajectory";
    }

};

#endif /* CLOPEMAMOVE_H_ */

The most of functions only call appropriate service and return results but there are also few interesting parts:

Get joints in group

This function loads robot model from the parameter server and returns all joints for specified group so it is not need to specify joints manually like in beginner tutorials.

std::vector<std::string> ClopemaMove::getJointsInGroup(std::string group) {
    planning_environment::CollisionModels cm("robot_description");
    return cm.getKinematicModel()->getModelGroup(group)->getJointModelNames();
}

Do goal and done callback

These two functions demonstrate another way of using action lib library. Instead of waiting for result we register callback function which will be called when action is done. Note that global variable cmc_ is action lib simple client which is initialized in constructor.

void ClopemaMove::doGoal(const control_msgs::FollowJointTrajectoryGoal & goal) {
    cmc_.sendGoal(goal, boost::bind(&ClopemaMove::doneCb, this, _1, _2));
    if (!cmc_.waitForResult(ros::Duration(30)))
        cmc_.cancelGoal();
}

void ClopemaMove::doneCb(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr& result) {
    ROS_INFO("Finished in state [%s:%s]", state.toString().c_str(), state.text_.data());
    ROS_INFO("Result - error code: %d", result->error_code);
    setServoPowerOff(false);
}

ClopemaMove::ClopemaMove() :
        cmc_(getServerName().data(), true) {
    if (!cmc_.waitForServer(ros::Duration(60))) {
        ROS_ERROR("Can't connect to server: %s", getServerName().data());
        ros::shutdown();
    }

}