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
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059 #include <ros/ros.h>
00060 #include <urdf/model.h>
00061 #include <actionlib/server/simple_action_server.h>
00062
00063
00064 #include <sensor_msgs/JointState.h>
00065 #include <control_msgs/FollowJointTrajectoryAction.h>
00066 #include <control_msgs/JointTrajectoryControllerState.h>
00067 #include <diagnostic_msgs/DiagnosticArray.h>
00068
00069
00070 #include <cob_srvs/Trigger.h>
00071 #include <cob_srvs/SetOperationMode.h>
00072 #include <cob_srvs/SetDefaultVel.h>
00073
00074
00075 #include <cob_head_axis/ElmoCtrl.h>
00076
00077 #include <unistd.h>
00078
00079
00080
00081 class NodeClass
00082 {
00083
00084 public:
00085
00086 ros::NodeHandle n_;
00087
00088
00089 ros::Publisher topicPub_JointState_;
00090 ros::Publisher topicPub_ControllerState_;
00091 ros::Publisher topicPub_Diagnostic_;
00092
00093
00094 ros::Subscriber topicSub_JointCommand_;
00095
00096
00097 ros::ServiceServer srvServer_Init_;
00098 ros::ServiceServer srvServer_Stop_;
00099 ros::ServiceServer srvServer_Recover_;
00100 ros::ServiceServer srvServer_SetOperationMode_;
00101 ros::ServiceServer srvServer_SetDefaultVel_;
00102
00103
00104
00105
00106
00107 actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00108 std::string action_name_;
00109 control_msgs::FollowJointTrajectoryFeedback feedback_;
00110 control_msgs::FollowJointTrajectoryResult result_;
00111
00112
00113 ElmoCtrl * CamAxis_;
00114 ElmoCtrlParams* CamAxisParams_;
00115
00116 std::string CanDevice_;
00117 std::string CanIniFile_;
00118 int CanBaudrate_;
00119 int HomingDir_;
00120 int HomingDigIn_;
00121 double MaxVel_;
00122 int ModID_;
00123 std::string operationMode_;
00124 double LowerLimit_;
00125 double UpperLimit_;
00126 double Offset_;
00127 int MotorDirection_;
00128 int EnoderIncrementsPerRevMot_;
00129 double GearRatio_;
00130
00131 std::string JointName_;
00132 bool isInitialized_;
00133 bool isError_;
00134 bool finished_;
00135 double ActualPos_;
00136 double ActualVel_;
00137 double GoalPos_;
00138 trajectory_msgs::JointTrajectory traj_;
00139 trajectory_msgs::JointTrajectoryPoint traj_point_;
00140 unsigned int traj_point_nr_;
00141
00142
00143 NodeClass(std::string name):
00144 as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1), false),
00145 action_name_(name)
00146 {
00147 n_ = ros::NodeHandle("~");
00148 as_.start();
00149
00150 isInitialized_ = false;
00151 isError_ = false;
00152 ActualPos_=0.0;
00153 ActualVel_=0.0;
00154
00155 CamAxis_ = new ElmoCtrl();
00156 CamAxisParams_ = new ElmoCtrlParams();
00157
00158
00159 topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00160 topicPub_ControllerState_ = n_.advertise<control_msgs::JointTrajectoryControllerState>("state", 1);
00161 topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00162
00163
00164
00165
00166
00167 srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
00168 srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
00169 srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
00170 srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
00171 srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
00172
00173
00174
00175
00176
00177 if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");
00178
00179 n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
00180 n_.param<int>("CanBaudrate", CanBaudrate_, 500);
00181 n_.param<int>("HomingDir", HomingDir_, 1);
00182 n_.param<int>("HomingDigIn", HomingDigIn_, 11);
00183 n_.param<int>("ModId",ModID_, 17);
00184 n_.param<std::string>("JointName",JointName_, "head_axis_joint");
00185 n_.param<std::string>("CanIniFile",CanIniFile_, "/");
00186 n_.param<std::string>("operation_mode",operationMode_, "position");
00187 n_.param<int>("MotorDirection",MotorDirection_, 1);
00188 n_.param<double>("GearRatio",GearRatio_, 62.5);
00189 n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
00190
00191 ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
00192
00193
00194
00195 std::string param_name = "/robot_description";
00196 std::string full_param_name;
00197 std::string xml_string;
00198 n_.searchParam(param_name,full_param_name);
00199 n_.getParam(full_param_name.c_str(),xml_string);
00200 ROS_INFO("full_param_name=%s",full_param_name.c_str());
00201 if (xml_string.size()==0)
00202 {
00203 ROS_ERROR("Unable to load robot model from param server robot_description\n");
00204 exit(2);
00205 }
00206 ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00207
00208
00209 urdf::Model model;
00210 if (!model.initString(xml_string))
00211 {
00212 ROS_ERROR("Failed to parse urdf file");
00213 exit(2);
00214 }
00215 ROS_INFO("Successfully parsed urdf file");
00216
00217
00218 LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
00219
00220 CamAxisParams_->SetLowerLimit(LowerLimit_);
00221
00222
00223 UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
00224
00225 CamAxisParams_->SetUpperLimit(UpperLimit_);
00226
00227
00228 Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
00229
00230 CamAxisParams_->SetAngleOffset(Offset_);
00231
00232
00233 MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
00234 ROS_INFO("Successfully read limits from urdf");
00235
00236
00237
00238 CamAxisParams_->SetCanIniFile(CanIniFile_);
00239 CamAxisParams_->SetHomingDir(HomingDir_);
00240 CamAxisParams_->SetHomingDigIn(HomingDigIn_);
00241 CamAxisParams_->SetMaxVel(MaxVel_);
00242 CamAxisParams_->SetGearRatio(GearRatio_);
00243 CamAxisParams_->SetMotorDirection(MotorDirection_);
00244 CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
00245
00246
00247
00248
00249 CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
00250
00251
00252 }
00253
00254
00255 ~NodeClass()
00256 {
00257 delete CamAxis_;
00258 }
00259
00260 void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
00261 if(isInitialized_) {
00262 ROS_INFO("Received new goal trajectory with %lu points",goal->trajectory.points.size());
00263
00264 traj_ = goal->trajectory;
00265 traj_point_nr_ = 0;
00266 traj_point_ = traj_.points[traj_point_nr_];
00267 GoalPos_ = traj_point_.positions[0];
00268 finished_ = false;
00269
00270
00271 CamAxis_->Stop();
00272
00273
00274 if (as_.isPreemptRequested())
00275 {
00276 ROS_INFO("%s: Preempted", action_name_.c_str());
00277
00278 as_.setPreempted();
00279 }
00280
00281 usleep(2000000);
00282
00283 while (not finished_)
00284 {
00285 if (as_.isNewGoalAvailable())
00286 {
00287 ROS_WARN("%s: Aborted", action_name_.c_str());
00288 as_.setAborted();
00289 return;
00290 }
00291 usleep(10000);
00292
00293
00294 }
00295
00296
00297
00298 ROS_INFO("%s: Succeeded", action_name_.c_str());
00299
00300 as_.setSucceeded(result_);
00301
00302 } else {
00303 as_.setAborted();
00304 ROS_WARN("Camera_axis not initialized yet!");
00305 }
00306 }
00307
00308
00309
00310 bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00311 cob_srvs::Trigger::Response &res )
00312 {
00313 if (isInitialized_ == false) {
00314 ROS_INFO("...initializing camera axis...");
00315
00316 if (CamAxis_->Init(CamAxisParams_))
00317 {
00318 CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
00319 ROS_INFO("Initializing of camera axis successfully");
00320 isInitialized_ = true;
00321 res.success.data = true;
00322 res.error_message.data = "initializing camera axis successfully";
00323 }
00324 else
00325 {
00326 ROS_ERROR("Initializing camera axis not successfully \n");
00327 res.success.data = false;
00328 res.error_message.data = "initializing camera axis not successfully";
00329 }
00330 }
00331 else
00332 {
00333 ROS_WARN("...camera axis already initialized...");
00334 res.success.data = true;
00335 res.error_message.data = "camera axis already initialized";
00336 }
00337
00338 return true;
00339 }
00340
00341 bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
00342 cob_srvs::Trigger::Response &res )
00343 {
00344 ROS_INFO("Stopping camera axis");
00345 if(isInitialized_)
00346 {
00347
00348 if (CamAxis_->Stop()) {
00349 ROS_INFO("Stopping camera axis successfully");
00350 res.success.data = true;
00351 res.error_message.data = "camera axis stopped successfully";
00352 }
00353 else {
00354 ROS_ERROR("Stopping camera axis not successfully. error");
00355 res.success.data = false;
00356 res.error_message.data = "stopping camera axis not successfully";
00357 }
00358 }
00359 return true;
00360 }
00361
00362 bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
00363 cob_srvs::Trigger::Response &res )
00364 {
00365 if (isInitialized_) {
00366 ROS_INFO("Recovering camera axis");
00367
00368
00369 if (CamAxis_->RecoverAfterEmergencyStop()) {
00370 ROS_INFO("Recovering camera axis successfully");
00371 res.success.data = true;
00372 res.error_message.data = "camera axis successfully recovered";
00373 } else {
00374 ROS_ERROR("Recovering camera axis not successfully. error");
00375 res.success.data = false;
00376 res.error_message.data = "recovering camera axis failed";
00377 }
00378 } else {
00379 ROS_WARN("...camera axis already recovered...");
00380 res.success.data = true;
00381 res.error_message.data = "camera axis already recovered";
00382 }
00383
00384 return true;
00385 }
00386
00394 bool srvCallback_SetOperationMode( cob_srvs::SetOperationMode::Request &req,
00395 cob_srvs::SetOperationMode::Response &res )
00396 {
00397 ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
00398 n_.setParam("operation_mode", req.operation_mode.data.c_str());
00399 res.success.data = true;
00400 return true;
00401 }
00402
00410 bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
00411 cob_srvs::SetDefaultVel::Response &res )
00412 {
00413 ROS_INFO("Set default velocity to [%f]", req.default_vel);
00414 MaxVel_ = req.default_vel;
00415 CamAxisParams_->SetMaxVel(MaxVel_);
00416 CamAxis_->setMaxVelocity(MaxVel_);
00417 res.success.data = true;
00418 return true;
00419 }
00420
00421
00422 void updateCommands()
00423 {
00424 if (isInitialized_ == true)
00425 {
00426 if (operationMode_ == "position")
00427 {
00428 ROS_DEBUG("moving head_axis in position mode");
00429
00430 if (fabs(ActualVel_) < 0.02)
00431 {
00432
00433
00434 ROS_DEBUG("next point is %d from %lu",traj_point_nr_,traj_.points.size());
00435
00436 if (traj_point_nr_ < traj_.points.size())
00437 {
00438
00439 ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
00440 traj_point_ = traj_.points[traj_point_nr_];
00441 CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
00442 usleep(900000);
00443 CamAxis_->m_Joint->requestPosVel();
00444 traj_point_nr_++;
00445
00446
00447
00448 }
00449 else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
00450 {
00451 ROS_DEBUG("...reached end of trajectory");
00452 finished_ = true;
00453 }
00454 else
00455 {
00456
00457 }
00458 }
00459 else
00460 {
00461 ROS_DEBUG("...axis still moving to point[%d]",traj_point_nr_);
00462 }
00463 }
00464 else if (operationMode_ == "velocity")
00465 {
00466 ROS_WARN("Moving in velocity mode currently disabled");
00467 }
00468 else
00469 {
00470 ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
00471 }
00472 }
00473 else
00474 {
00475 ROS_DEBUG("axis not initialized");
00476 }
00477 }
00478
00479 void publishJointState()
00480 {
00481
00482 if (isInitialized_ == true) {
00483 isError_ = CamAxis_->isError();
00484
00485
00486 int DOF = 1;
00487
00488 CamAxis_->evalCanBuffer();
00489 CamAxis_->getGearPosVelRadS(&ActualPos_,&ActualVel_);
00490 CamAxis_->m_Joint->requestPosVel();
00491
00492
00493 ActualPos_ = HomingDir_ * ActualPos_;
00494 ActualVel_ = HomingDir_ * ActualVel_;
00495
00496 sensor_msgs::JointState msg;
00497 msg.header.stamp = ros::Time::now();
00498 msg.name.resize(DOF);
00499 msg.position.resize(DOF);
00500 msg.velocity.resize(DOF);
00501 msg.effort.resize(DOF);
00502
00503 msg.name[0] = JointName_;
00504 msg.position[0] = ActualPos_;
00505 msg.velocity[0] = ActualVel_;
00506 msg.effort[0] = 0.0;
00507
00508
00509
00510
00511
00512 topicPub_JointState_.publish(msg);
00513
00514
00515 control_msgs::JointTrajectoryControllerState controllermsg;
00516 controllermsg.header = msg.header;
00517 controllermsg.joint_names.resize(DOF);
00518 controllermsg.desired.positions.resize(DOF);
00519 controllermsg.desired.velocities.resize(DOF);
00520 controllermsg.actual.positions.resize(DOF);
00521 controllermsg.actual.velocities.resize(DOF);
00522 controllermsg.error.positions.resize(DOF);
00523 controllermsg.error.velocities.resize(DOF);
00524
00525 controllermsg.joint_names = msg.name;
00526 controllermsg.desired.positions = msg.position;
00527 controllermsg.desired.velocities = msg.velocity;
00528 controllermsg.actual.positions = msg.position;
00529 controllermsg.actual.velocities = msg.velocity;
00530
00531 for (int i = 0; i<DOF; i++ )
00532 {
00533 controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
00534 controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
00535 }
00536 topicPub_ControllerState_.publish(controllermsg);
00537 }
00538
00539 diagnostic_msgs::DiagnosticArray diagnostics;
00540 diagnostics.status.resize(1);
00541
00542 if(isError_)
00543 {
00544 diagnostics.status[0].level = 2;
00545 diagnostics.status[0].name = n_.getNamespace();
00546 diagnostics.status[0].message = "drive is in error mode";
00547 }
00548 else
00549 {
00550 if (isInitialized_)
00551 {
00552 diagnostics.status[0].level = 0;
00553 diagnostics.status[0].name = n_.getNamespace();
00554 diagnostics.status[0].message = "head axis initialized and running";
00555 }
00556 else
00557 {
00558 diagnostics.status[0].level = 1;
00559 diagnostics.status[0].name = n_.getNamespace();
00560 diagnostics.status[0].message = "head axis not initialized";
00561 }
00562 }
00563
00564 topicPub_Diagnostic_.publish(diagnostics);
00565 }
00566
00567 };
00568
00569
00570
00571
00572 int main(int argc, char** argv)
00573 {
00574
00575 ros::init(argc, argv, "cob_camera_axis");
00576
00577
00578
00579 NodeClass nodeClass(ros::this_node::getName() + "/follow_joint_trajectory");
00580
00581
00582 ros::Rate loop_rate(10);
00583 while(nodeClass.n_.ok()) {
00584
00585
00586 nodeClass.publishJointState();
00587
00588
00589 nodeClass.updateCommands();
00590
00591
00592 ros::spinOnce();
00593 loop_rate.sleep();
00594 }
00595
00596 return 0;
00597 }
00598