/*! * \brief Executes the service callback for recover. * * Recovers the driver after an emergency stop. * \param req Service request * \param res Service response */ bool srvCallback_Recover( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res ) { if (isInitialized_ == true) { ROS_INFO("Recovering powercubes"); // stopping all arm movements if (PCube_->Stop()) { ROS_INFO("Recovering powercubes succesfull"); res.success.data = true; } else { ROS_ERROR("Recovering powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str()); res.success.data = false; res.error_message.data = PCube_->getErrorMessage(); } } else { ROS_ERROR("...powercubes already recovered..."); res.success.data = false; res.error_message.data = "powercubes already recovered"; } return true; }
/*! * \brief Executes the service callback for init. * * Connects to the hardware and initialized it. * \param req Service request * \param res Service response */ bool srvCallback_Init( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res ) { if (isInitialized_ == false) { ROS_INFO("...initializing powercubes..."); // init powercubes if (PCube_->Init(PCubeParams_)) { ROS_INFO("Initializing succesfull"); isInitialized_ = true; res.success.data = true; res.error_message.data = "success"; } else { ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str()); res.success.data = false; res.error_message.data = PCube_->getErrorMessage(); } } else { ROS_ERROR("...powercubes already initialized..."); res.success.data = false; res.error_message.data = "powercubes already initialized"; } return true; }
/*! * \brief Executes the service callback for recover. * * Recovers the driver after an emergency stop. * \param req Service request * \param res Service response */ bool srvCallback_Recover(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res) { ROS_INFO("Recovering powercubes..."); if (initialized_) { /// stopping all arm movements if (pc_ctrl_->Recover()) { error_ = false; error_msg_ = ""; res.success.data = true; ROS_INFO("...recovering powercubes successful."); } else { res.success.data = false; error_ = true; error_msg_ = pc_ctrl_->getErrorMessage(); res.error_message.data = pc_ctrl_->getErrorMessage(); ROS_ERROR("...recovering powercubes not successful. error: %s", res.error_message.data.c_str()); } } else { res.success.data = false; res.error_message.data = "powercubes not initialized"; ROS_ERROR("...recovering powercubes not successful. error: %s",res.error_message.data.c_str()); } return true; }
/*! * \brief Executes the service callback for init. * * Connects to the hardware and initialized it. * \param req Service request * \param res Service response */ bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res) { if (!initialized_) { ROS_INFO("Initializing powercubes..."); /// initialize powercubes if (pc_ctrl_->Init(pc_params_)) { initialized_ = true; res.success.data = true; ROS_INFO("...initializing powercubes successful"); } else { error_ = true; error_msg_ = pc_ctrl_->getErrorMessage(); res.success.data = false; res.error_message.data = pc_ctrl_->getErrorMessage(); ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str()); } } else { res.success.data = true; res.error_message.data = "powercubes already initialized"; ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str()); } return true; }
bool srvCallback_Stop(cob3_srvs::Stop::Request &req, cob3_srvs::Stop::Response &res ) { ROS_INFO("Stopping powercubes"); // stopping all arm movements if (PCube->Stop()) { ROS_INFO("Stopping powercubes succesfull"); res.success = 0; // 0 = true, else = false } else { ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str()); res.success = 1; // 0 = true, else = false res.errorMessage.data = PCube->getErrorMessage(); } return true; }
/*! * \brief Executes the service callback for stop. * * Stops all hardware movements. * \param req Service request * \param res Service response */ bool srvCallback_Stop( cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res ) { ROS_INFO("Stopping powercubes"); newvel_ = false; // set current trajectory to be finished traj_point_nr_ = traj_.points.size(); // stopping all arm movements if (PCube_->Stop()) { ROS_INFO("Stopping powercubes succesfull"); res.success.data = true; } else { ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str()); res.success.data = false; res.error_message.data = PCube_->getErrorMessage(); } return true; }
// service callback functions // function will be called when a service is querried bool srvCallback_Init(cob3_srvs::Init::Request &req, cob3_srvs::Init::Response &res ) { ROS_INFO("Initializing powercubes"); // init powercubes //TODO: make iniFilepath as an argument //TODO: read iniFile into ros prarameters if (PCube->Init("include/iniFile.txt")) { ROS_INFO("Initializing succesfull"); res.success = 0; // 0 = true, else = false } else { ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str()); res.success = 1; // 0 = true, else = false res.errorMessage.data = PCube->getErrorMessage(); return true; } // homing powercubes if (PCube->doHoming()) { ROS_INFO("Homing powercubes succesfull"); res.success = 0; // 0 = true, else = false } else { ROS_ERROR("Homing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str()); res.success = 1; // 0 = true, else = false res.errorMessage.data = PCube->getErrorMessage(); return true; } return true; }
/*! * \brief Executes the service callback for stop. * * Stops all hardware movements. * \param req Service request * \param res Service response */ bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res) { ROS_INFO("Stopping powercubes..."); // stop powercubes if (pc_ctrl_->Stop()) { res.success.data = true; ROS_INFO("...stopping powercubes successful."); } else { res.success.data = false; res.error_message.data = pc_ctrl_->getErrorMessage(); ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str()); } return true; }
/*! * \brief Publishes the state of the powercube_chain as ros messages. * * Published to "/joint_states" as "sensor_msgs/JointState" * Published to "state" as "pr2_controllers_msgs/JointTrajectoryState" */ void publishState(bool update=true) { if (initialized_) { ROS_DEBUG("publish state"); if(update) {pc_ctrl_->updateStates();} sensor_msgs::JointState joint_state_msg; joint_state_msg.header.stamp = ros::Time::now(); joint_state_msg.name = pc_params_->GetJointNames(); joint_state_msg.position = pc_ctrl_->getPositions(); joint_state_msg.velocity = pc_ctrl_->getVelocities(); joint_state_msg.effort.resize(pc_params_->GetDOF()); pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg; controller_state_msg.header.stamp = joint_state_msg.header.stamp; controller_state_msg.joint_names = pc_params_->GetJointNames(); controller_state_msg.actual.positions = pc_ctrl_->getPositions(); controller_state_msg.actual.velocities = pc_ctrl_->getVelocities(); controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations(); std_msgs::String opmode_msg; opmode_msg.data = "velocity"; /// publishing joint and controller states on topic topicPub_JointState_.publish(joint_state_msg); topicPub_ControllerState_.publish(controller_state_msg); topicPub_OperationMode_.publish(opmode_msg); last_publish_time_ = joint_state_msg.header.stamp; } // check status of PowerCube chain if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; } // check status of PowerCube chain if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; } else { error_ = false; } // publishing diagnotic messages diagnostic_msgs::DiagnosticArray diagnostics; diagnostics.status.resize(1); // set data to diagnostics if(error_) { diagnostics.status[0].level = 2; diagnostics.status[0].name = n_.getNamespace();; diagnostics.status[0].message = pc_ctrl_->getErrorMessage(); } else { if (initialized_) { diagnostics.status[0].level = 0; diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain"; diagnostics.status[0].message = "powercubechain initialized and running"; } else { diagnostics.status[0].level = 1; diagnostics.status[0].name = n_.getNamespace(); //"schunk_powercube_chain"; diagnostics.status[0].message = "powercubechain not initialized"; } } // publish diagnostic message topicPub_Diagnostic_.publish(diagnostics); }
/*! * \brief Executes the callback from the command_vel topic. * * Set the current velocity target. * \param msg JointVelocities */ void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg) { ROS_DEBUG("Received new velocity command"); if (!initialized_) { ROS_WARN("Skipping command: powercubes not initialized"); publishState(false); return; } if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { publishState(false); return; } PowerCubeCtrl::PC_CTRL_STATUS status; std::vector<std::string> errorMessages; pc_ctrl_->getStatus(status, errorMessages); /// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates) unsigned int DOF = pc_params_->GetDOF(); std::vector<std::string> jointNames = pc_params_->GetJointNames(); std::vector<double> cmd_vel(DOF); std::string unit = "rad"; /// check dimensions if (msg->velocities.size() != DOF) { ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension."); return; } /// parse velocities for (unsigned int i = 0; i < DOF; i++) { /// check joint name if (msg->velocities[i].joint_uri != jointNames[i]) { ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i); return; } /// check unit if (msg->velocities[i].unit != unit) { ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str()); return; } /// if all checks are successful, parse the velocity value for this joint ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str()); cmd_vel[i] = msg->velocities[i].value; } /// command velocities to powercubes if (!pc_ctrl_->MoveVel(cmd_vel)) { error_ = true; error_msg_ = pc_ctrl_->getErrorMessage(); ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str()); return; } ROS_DEBUG("Executed velocity command"); publishState(false); }