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)

/* 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 <youbot_oodl/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 <youbot_oodl/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;
  youbot_oodl::PowerBoardState platformStateMessage;

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

  bool areBaseMotorsSwitchedOn;
  bool areArmMotorsSwitchedOn;
};

} // namespace youBot

#endif /* YOUBOTOODLWRAPPER_H_ */

/* EOF */