YouBotOODLWrapper.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright (c) 2011
 * Locomotec
 *
 * Author:
 * Sebastian Blumenthal
 *
 *
 * This software is published under a dual-license: GNU Lesser General Public
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * 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 Locomotec nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD 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 Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ******************************************************************************/

#ifndef YOUBOTOODLWRAPPER_H_
#define YOUBOTOODLWRAPPER_H_

/* Stringification helper macros */
#define mkstr2(X) #X
#define mkstr(X) mkstr2(X)

/* BOOST includes */
#include <boost/units/io.hpp>

/* ROS includes */
#include "geometry_msgs/Twist.h"
#include "tf/transform_broadcaster.h"
#include "nav_msgs/Odometry.h"
#include "std_srvs/Empty.h"
#include "diagnostic_msgs/DiagnosticStatus.h"
#include <diagnostic_msgs/DiagnosticArray.h>

#include <pr2_msgs/PowerBoardState.h>

#include "trajectory_msgs/JointTrajectory.h"
#include "sensor_msgs/JointState.h"

#include "brics_actuator/JointPositions.h"
#include "brics_actuator/JointVelocities.h"

/* OODL includes */
#include "YouBotConfiguration.h"
#include <youbot_driver/youbot/JointTrajectoryController.hpp>
#include <youbot_driver/youbot/DataTrace.hpp>

//#include <control_msgs/FollowJointTrajectoryAction.h>
//#include <actionlib/server/simple_action_server.h>

//typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

namespace youBot
{

class YouBotOODLWrapper
{
public:

    YouBotOODLWrapper(ros::NodeHandle n);

    virtual ~YouBotOODLWrapper();


    /* Coordination: */

    void initializeBase(std::string baseName);

    void initializeArm(std::string armName, bool enableStandardGripper = true);

    void stop();


    /* Communication: */

    void baseCommandCallback(const geometry_msgs::Twist& youbotBaseCommand);

    void armCommandCallback(const trajectory_msgs::JointTrajectory& youbotArmCommand);

    void armPositionsCommandCallback(const brics_actuator::JointPositionsConstPtr& youbotArmCommand, int armIndex);

    void armVelocitiesCommandCallback(const brics_actuator::JointVelocitiesConstPtr& youbotArmCommand, int armIndex);

    void armJointTrajectoryGoalCallback(actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex);

    void armJointTrajectoryCancelCallback(actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex);
    
    void gripperPositionsCommandCallback(const brics_actuator::JointPositionsConstPtr& youbotGripperCommand, int armIndex);

    void publishOODLSensorReadings();
    
    void publishArmAndBaseDiagnostics(double publish_rate_in_secs);

    /* Computation: */

    void computeOODLSensorReadings();

    bool switchOffBaseMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

    bool switchOnBaseMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

    bool switchOffArmMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);

    bool switchOnArmMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);

    bool calibrateArmCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);

    bool reconnectCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);

    /* Configuration: */

    YouBotConfiguration youBotConfiguration;

private:

    YouBotOODLWrapper(); //forbid default constructor
    

    static const int youBotArmDoF = 5;

    static const int youBotNumberOfFingers = 2;

    static const int youBotNumberOfWheels = 4;


    std::string youBotChildFrameID;
    std::string youBotOdometryFrameID;
    std::string youBotOdometryChildFrameID;
    std::string youBotArmFrameID;


    ros::NodeHandle node;

    ros::Time currentTime;


    nav_msgs::Odometry odometryMessage;

    geometry_msgs::TransformStamped odometryTransform;

    geometry_msgs::Quaternion odometryQuaternion;

    sensor_msgs::JointState baseJointStateMessage;

    vector<sensor_msgs::JointState> armJointStateMessages;

    actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle armActiveJointTrajectoryGoal;

    bool armHasActiveJointTrajectoryGoal;

    youbot::GripperSensedBarPosition gripperBar1Position;
    youbot::GripperSensedBarPosition gripperBar2Position;
    int gripperCycleCounter;

    //void executeActionServer(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal,  int armIndex);
    
    //bool trajectoryActionServerEnable;
    //double trajectoryVelocityGain;
    //double trajectoryPositionGain;
    double youBotDriverCycleFrequencyInHz;
        
    ros::Time lastDiagnosticPublishTime;

    ros::Publisher dashboardMessagePublisher;
    pr2_msgs::PowerBoardState platformStateMessage;

    ros::Publisher diagnosticArrayPublisher;
    diagnostic_msgs::DiagnosticArray diagnosticArrayMessage;
    diagnostic_msgs::DiagnosticStatus diagnosticStatusMessage;
    std::string diagnosticNameArm;
    std::string diagnosticNameBase;

    bool areBaseMotorsSwitchedOn;
    bool areArmMotorsSwitchedOn;
};

} // namespace youBot

#endif /* YOUBOTOODLWRAPPER_H_ */

/* EOF */