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
00066 #include <control_msgs/FollowJointTrajectoryAction.h>
00067 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00068 #include <diagnostic_msgs/DiagnosticArray.h>
00069
00070
00071 #include <cob_srvs/Trigger.h>
00072 #include <cob_srvs/SetOperationMode.h>
00073 #include <cob_srvs/SetDefaultVel.h>
00074
00075
00076 #include <cob_camera_axis/ElmoCtrl.h>
00077
00078
00079
00080 class NodeClass
00081 {
00082
00083 public:
00084
00085 ros::NodeHandle n_;
00086
00087
00088 ros::Publisher topicPub_JointState_;
00089 ros::Publisher topicPub_ControllerState_;
00090 ros::Publisher topicPub_Diagnostic_;
00091
00092
00093 ros::Subscriber topicSub_JointCommand_;
00094
00095
00096 ros::ServiceServer srvServer_Init_;
00097 ros::ServiceServer srvServer_Stop_;
00098 ros::ServiceServer srvServer_Recover_;
00099 ros::ServiceServer srvServer_SetOperationMode_;
00100 ros::ServiceServer srvServer_SetDefaultVel_;
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00111 std::string action_name_;
00112 control_msgs::FollowJointTrajectoryFeedback feedback_;
00113 control_msgs::FollowJointTrajectoryResult result_;
00114
00115
00116 ElmoCtrl * CamAxis_;
00117 ElmoCtrlParams* CamAxisParams_;
00118
00119 std::string CanDevice_;
00120 std::string CanIniFile_;
00121 int CanBaudrate_;
00122 int HomingDir_;
00123 int HomingDigIn_;
00124 double MaxVel_;
00125 int ModID_;
00126 std::string operationMode_;
00127 double LowerLimit_;
00128 double UpperLimit_;
00129 double Offset_;
00130 int MotorDirection_;
00131 int EnoderIncrementsPerRevMot_;
00132 double GearRatio_;
00133
00134 std::string JointName_;
00135 bool isInitialized_;
00136 bool isError_;
00137 bool finished_;
00138 double ActualPos_;
00139 double ActualVel_;
00140 double GoalPos_;
00141 trajectory_msgs::JointTrajectory traj_;
00142 trajectory_msgs::JointTrajectoryPoint traj_point_;
00143 unsigned int traj_point_nr_;
00144
00145
00146 NodeClass(std::string name):
00147 as_(n_, name, boost::bind(&NodeClass::executeCB, this, _1)),
00148 action_name_(name)
00149 {
00150 n_ = ros::NodeHandle("~");
00151
00152 isInitialized_ = false;
00153 isError_ = false;
00154 ActualPos_=0.0;
00155 ActualVel_=0.0;
00156
00157 CamAxis_ = new ElmoCtrl();
00158 CamAxisParams_ = new ElmoCtrlParams();
00159
00160
00161 topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("/joint_states", 1);
00162 topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
00163 topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00164
00165
00166
00167
00168
00169 srvServer_Init_ = n_.advertiseService("init", &NodeClass::srvCallback_Init, this);
00170 srvServer_Stop_ = n_.advertiseService("stop", &NodeClass::srvCallback_Stop, this);
00171 srvServer_Recover_ = n_.advertiseService("recover", &NodeClass::srvCallback_Recover, this);
00172 srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &NodeClass::srvCallback_SetOperationMode, this);
00173 srvServer_SetDefaultVel_ = n_.advertiseService("set_default_vel", &NodeClass::srvCallback_SetDefaultVel, this);
00174
00175
00176
00177
00178
00179 if(!n_.hasParam("EnoderIncrementsPerRevMot")) ROS_WARN("cob_head_axis: couldn't find parameter EnoderIncrementsPerRevMot, check if ALL parameters have been set correctly");
00180
00181 n_.param<std::string>("CanDevice", CanDevice_, "PCAN");
00182 n_.param<int>("CanBaudrate", CanBaudrate_, 500);
00183 n_.param<int>("HomingDir", HomingDir_, 1);
00184 n_.param<int>("HomingDigIn", HomingDigIn_, 11);
00185 n_.param<int>("ModId",ModID_, 17);
00186 n_.param<std::string>("JointName",JointName_, "head_axis_joint");
00187 n_.param<std::string>("CanIniFile",CanIniFile_, "/");
00188 n_.param<std::string>("operation_mode",operationMode_, "position");
00189 n_.param<int>("MotorDirection",MotorDirection_, 1);
00190 n_.param<double>("GearRatio",GearRatio_, 62.5);
00191 n_.param<int>("EnoderIncrementsPerRevMot",EnoderIncrementsPerRevMot_, 4096);
00192
00193 ROS_INFO("CanDevice=%s, CanBaudrate=%d, ModID=%d, HomingDigIn=%d",CanDevice_.c_str(),CanBaudrate_,ModID_,HomingDigIn_);
00194
00195
00196
00197 std::string param_name = "/robot_description";
00198 std::string full_param_name;
00199 std::string xml_string;
00200 n_.searchParam(param_name,full_param_name);
00201 n_.getParam(full_param_name.c_str(),xml_string);
00202 ROS_INFO("full_param_name=%s",full_param_name.c_str());
00203 if (xml_string.size()==0)
00204 {
00205 ROS_ERROR("Unable to load robot model from param server robot_description\n");
00206 exit(2);
00207 }
00208 ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00209
00210
00211 urdf::Model model;
00212 if (!model.initString(xml_string))
00213 {
00214 ROS_ERROR("Failed to parse urdf file");
00215 exit(2);
00216 }
00217 ROS_INFO("Successfully parsed urdf file");
00218
00219
00220 LowerLimit_ = model.getJoint(JointName_.c_str())->limits->lower;
00221
00222 CamAxisParams_->SetLowerLimit(LowerLimit_);
00223
00224
00225 UpperLimit_ = model.getJoint(JointName_.c_str())->limits->upper;
00226
00227 CamAxisParams_->SetUpperLimit(UpperLimit_);
00228
00229
00230 Offset_ = model.getJoint(JointName_.c_str())->calibration->rising.get()[0];
00231
00232 CamAxisParams_->SetAngleOffset(Offset_);
00233
00234
00235 MaxVel_ = model.getJoint(JointName_.c_str())->limits->velocity;
00236 ROS_INFO("Successfully read limits from urdf");
00237
00238
00239
00240 CamAxisParams_->SetCanIniFile(CanIniFile_);
00241 CamAxisParams_->SetHomingDir(HomingDir_);
00242 CamAxisParams_->SetHomingDigIn(HomingDigIn_);
00243 CamAxisParams_->SetMaxVel(MaxVel_);
00244 CamAxisParams_->SetGearRatio(GearRatio_);
00245 CamAxisParams_->SetMotorDirection(MotorDirection_);
00246 CamAxisParams_->SetEncoderIncrements(EnoderIncrementsPerRevMot_);
00247
00248
00249
00250
00251 CamAxisParams_->Init(CanDevice_, CanBaudrate_, ModID_);
00252
00253
00254 }
00255
00256
00257 ~NodeClass()
00258 {
00259 delete CamAxis_;
00260 }
00261
00262
00263 void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
00264 if(isInitialized_) {
00265 ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
00266
00267 traj_ = goal->trajectory;
00268 traj_point_nr_ = 0;
00269 traj_point_ = traj_.points[traj_point_nr_];
00270 GoalPos_ = traj_point_.positions[0];
00271 finished_ = false;
00272
00273
00274 CamAxis_->Stop();
00275
00276
00277 if (as_.isPreemptRequested())
00278 {
00279 ROS_INFO("%s: Preempted", action_name_.c_str());
00280
00281 as_.setPreempted();
00282 }
00283
00284 usleep(2000000);
00285
00286 while (not finished_)
00287 {
00288 if (as_.isNewGoalAvailable())
00289 {
00290 ROS_WARN("%s: Aborted", action_name_.c_str());
00291 as_.setAborted();
00292 return;
00293 }
00294 usleep(10000);
00295
00296
00297 }
00298
00299
00300
00301 ROS_INFO("%s: Succeeded", action_name_.c_str());
00302
00303 as_.setSucceeded(result_);
00304
00305 } else {
00306 as_.setAborted();
00307 ROS_WARN("Camera_axis not initialized yet!");
00308 }
00309 }
00310
00311
00312
00313 bool srvCallback_Init(cob_srvs::Trigger::Request &req,
00314 cob_srvs::Trigger::Response &res )
00315 {
00316 if (isInitialized_ == false) {
00317 ROS_INFO("...initializing camera axis...");
00318
00319 if (CamAxis_->Init(CamAxisParams_))
00320 {
00321 CamAxis_->setGearPosVelRadS(0.0f, MaxVel_);
00322 ROS_INFO("Initializing of camera axis successfully");
00323 isInitialized_ = true;
00324 res.success.data = true;
00325 res.error_message.data = "initializing camera axis successfully";
00326 }
00327 else
00328 {
00329 ROS_ERROR("Initializing camera axis not successfully \n");
00330 res.success.data = false;
00331 res.error_message.data = "initializing camera axis not successfully";
00332 }
00333 }
00334 else
00335 {
00336 ROS_WARN("...camera axis already initialized...");
00337 res.success.data = true;
00338 res.error_message.data = "camera axis already initialized";
00339 }
00340
00341 return true;
00342 }
00343
00344 bool srvCallback_Stop(cob_srvs::Trigger::Request &req,
00345 cob_srvs::Trigger::Response &res )
00346 {
00347 ROS_INFO("Stopping camera axis");
00348 if(isInitialized_)
00349 {
00350
00351 if (CamAxis_->Stop()) {
00352 ROS_INFO("Stopping camera axis successfully");
00353 res.success.data = true;
00354 res.error_message.data = "camera axis stopped successfully";
00355 }
00356 else {
00357 ROS_ERROR("Stopping camera axis not successfully. error");
00358 res.success.data = false;
00359 res.error_message.data = "stopping camera axis not successfully";
00360 }
00361 }
00362 return true;
00363 }
00364
00365 bool srvCallback_Recover(cob_srvs::Trigger::Request &req,
00366 cob_srvs::Trigger::Response &res )
00367 {
00368 if (isInitialized_) {
00369 ROS_INFO("Recovering camera axis");
00370
00371
00372 if (CamAxis_->RecoverAfterEmergencyStop()) {
00373 ROS_INFO("Recovering camera axis successfully");
00374 res.success.data = true;
00375 res.error_message.data = "camera axis successfully recovered";
00376 } else {
00377 ROS_ERROR("Recovering camera axis not successfully. error");
00378 res.success.data = false;
00379 res.error_message.data = "recovering camera axis failed";
00380 }
00381 } else {
00382 ROS_WARN("...camera axis already recovered...");
00383 res.success.data = true;
00384 res.error_message.data = "camera axis already recovered";
00385 }
00386
00387 return true;
00388 }
00389
00397 bool srvCallback_SetOperationMode( cob_srvs::SetOperationMode::Request &req,
00398 cob_srvs::SetOperationMode::Response &res )
00399 {
00400 ROS_INFO("Set operation mode to [%s]", req.operation_mode.data.c_str());
00401 n_.setParam("operation_mode", req.operation_mode.data.c_str());
00402 res.success.data = true;
00403 return true;
00404 }
00405
00413 bool srvCallback_SetDefaultVel( cob_srvs::SetDefaultVel::Request &req,
00414 cob_srvs::SetDefaultVel::Response &res )
00415 {
00416 ROS_INFO("Set default velocity to [%f]", req.default_vel);
00417 MaxVel_ = req.default_vel;
00418 CamAxisParams_->SetMaxVel(MaxVel_);
00419 CamAxis_->setMaxVelocity(MaxVel_);
00420 res.success.data = true;
00421 return true;
00422 }
00423
00424
00425 void updateCommands()
00426 {
00427 if (isInitialized_ == true)
00428 {
00429 if (operationMode_ == "position")
00430 {
00431 ROS_DEBUG("moving head_axis in position mode");
00432
00433 if (fabs(ActualVel_) < 0.02)
00434 {
00435
00436
00437 ROS_DEBUG("next point is %d from %d",traj_point_nr_,traj_.points.size());
00438
00439 if (traj_point_nr_ < traj_.points.size())
00440 {
00441
00442 ROS_INFO("...moving to trajectory point[%d], %f",traj_point_nr_,traj_.points[traj_point_nr_].positions[0]);
00443 traj_point_ = traj_.points[traj_point_nr_];
00444 CamAxis_->setGearPosVelRadS(traj_point_.positions[0], MaxVel_);
00445 usleep(900000);
00446 CamAxis_->m_Joint->requestPosVel();
00447 traj_point_nr_++;
00448
00449
00450
00451 }
00452 else if ( fabs( fabs(ActualPos_) - fabs(GoalPos_) ) < 0.5*M_PI/180.0 && !finished_ )
00453 {
00454 ROS_DEBUG("...reached end of trajectory");
00455 finished_ = true;
00456 }
00457 else
00458 {
00459
00460 }
00461 }
00462 else
00463 {
00464 ROS_INFO("...axis still moving to point[%d]",traj_point_nr_);
00465 }
00466 }
00467 else if (operationMode_ == "velocity")
00468 {
00469 ROS_WARN("Moving in velocity mode currently disabled");
00470 }
00471 else
00472 {
00473 ROS_ERROR("axis neither in position nor in velocity mode. OperationMode = [%s]", operationMode_.c_str());
00474 }
00475 }
00476 else
00477 {
00478 ROS_DEBUG("axis not initialized");
00479 }
00480 }
00481
00482 void publishJointState()
00483 {
00484 if (isInitialized_ == true) {
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 pr2_controllers_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 = "head_axis";
00546 diagnostics.status[0].message = "one or more drives are 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