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