Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef YOUBOTOODLWRAPPER_H_
00041 #define YOUBOTOODLWRAPPER_H_
00042
00043
00044 #define mkstr2(X) #X
00045 #define mkstr(X) mkstr2(X)
00046
00047
00048 #include "geometry_msgs/Twist.h"
00049 #include "tf/transform_broadcaster.h"
00050 #include "nav_msgs/Odometry.h"
00051 #include "std_srvs/Empty.h"
00052 #include "diagnostic_msgs/DiagnosticStatus.h"
00053 #include <diagnostic_msgs/DiagnosticArray.h>
00054
00055 #include "youbot_common/PowerBoardState.h"
00056
00057 #include "trajectory_msgs/JointTrajectory.h"
00058 #include "sensor_msgs/JointState.h"
00059
00060 #include "brics_actuator/JointPositions.h"
00061 #include "brics_actuator/JointVelocities.h"
00062
00063
00064 #include "YouBotConfiguration.h"
00065 #include "youbot_driver/youbot/JointTrajectoryController.hpp"
00066 #include "youbot_driver/youbot/DataTrace.hpp"
00067
00068
00069
00070
00071
00072
00073 namespace youBot
00074 {
00075
00079 class YouBotOODLWrapper
00080 {
00081 public:
00082
00087 YouBotOODLWrapper(ros::NodeHandle n);
00088
00092 virtual ~YouBotOODLWrapper();
00093
00094
00095
00096
00101 void initializeBase(std::string baseName);
00102
00108 void initializeArm(std::string armName, bool enableStandardGripper = true);
00109
00114 void stop();
00115
00116
00117
00118
00123 void baseCommandCallback(const geometry_msgs::Twist& youbotBaseCommand);
00124
00133 void armCommandCallback(const trajectory_msgs::JointTrajectory& youbotArmCommand);
00134
00140 void armPositionsCommandCallback(const brics_actuator::JointPositionsConstPtr& youbotArmCommand, int armIndex);
00141
00147 void armVelocitiesCommandCallback(const brics_actuator::JointVelocitiesConstPtr& youbotArmCommand, int armIndex);
00148
00154 void armJointTrajectoryGoalCallback(actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex);
00155
00161 void armJointTrajectoryCancelCallback(actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle youbotArmGoal, unsigned int armIndex);
00162
00168 void gripperPositionsCommandCallback(const brics_actuator::JointPositionsConstPtr& youbotGripperCommand, int armIndex);
00169
00176 void publishOODLSensorReadings();
00177
00181 void publishArmAndBaseDiagnostics(double publish_rate_in_secs);
00182
00183
00184
00188 void computeOODLSensorReadings();
00189
00190 bool switchOffBaseMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00191
00192 bool switchOnBaseMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00193
00194 bool switchOffArmMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);
00195
00196 bool switchOnArmMotorsCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);
00197
00198 bool calibrateArmCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response, int armIndex);
00199
00200 bool reconnectCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00201
00202
00203
00205 YouBotConfiguration youBotConfiguration;
00206
00207 private:
00208
00209 YouBotOODLWrapper();
00210
00211
00213 static const int youBotArmDoF = 5;
00214
00216 static const int youBotNumberOfFingers = 2;
00217
00219 static const int youBotNumberOfWheels = 4;
00220
00221
00222 std::string youBotChildFrameID;
00223 std::string youBotOdometryFrameID;
00224 std::string youBotOdometryChildFrameID;
00225 std::string youBotArmFrameID;
00226
00227
00229 ros::NodeHandle node;
00230
00232 ros::Time currentTime;
00233
00234
00236 nav_msgs::Odometry odometryMessage;
00237
00239 geometry_msgs::TransformStamped odometryTransform;
00240
00242 geometry_msgs::Quaternion odometryQuaternion;
00243
00245 tf::Transform odometryOffset;
00247 double last_x,last_y,last_theta;
00248
00250 sensor_msgs::JointState baseJointStateMessage;
00251
00253 vector<sensor_msgs::JointState> armJointStateMessages;
00254
00256 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::GoalHandle armActiveJointTrajectoryGoal;
00257
00259 bool armHasActiveJointTrajectoryGoal;
00260
00261 youbot::GripperSensedBarPosition gripperBar1Position;
00262 youbot::GripperSensedBarPosition gripperBar2Position;
00263 int gripperCycleCounter;
00264
00265
00266
00267
00268
00269
00270 double youBotDriverCycleFrequencyInHz;
00271
00273 ros::Time lastDiagnosticPublishTime;
00274
00275 ros::Publisher dashboardMessagePublisher;
00276 youbot_common::PowerBoardState platformStateMessage;
00277
00278 ros::Publisher diagnosticArrayPublisher;
00279 diagnostic_msgs::DiagnosticArray diagnosticArrayMessage;
00280 diagnostic_msgs::DiagnosticStatus diagnosticStatusMessage;
00281 std::string diagnosticNameArms;
00282 std::string diagnosticNameBase;
00283
00284 bool areBaseMotorsSwitchedOn;
00285 bool areArmMotorsSwitchedOn;
00286 };
00287
00288 }
00289
00290 #endif
00291
00292