28 #include <std_msgs/Float64.h> 29 #include <sensor_msgs/JointState.h> 30 #include <diagnostic_msgs/DiagnosticStatus.h> 31 #include <diagnostic_msgs/DiagnosticArray.h> 32 #include <control_msgs/JointTrajectoryControllerState.h> 33 #include <control_msgs/JointControllerState.h> 36 #include <std_srvs/Trigger.h> 37 #include <cob_base_drive_chain/ElmoRecorderReadout.h> 38 #include <cob_base_drive_chain/ElmoRecorderConfig.h> 137 std::vector<double> m_gazeboPos;
138 std::vector<double> m_gazeboVel;
169 n.
getParam(
"IniDirectory", sIniDirectory);
170 ROS_INFO(
"IniDirectory loaded from Parameter-Server is: %s", sIniDirectory.c_str());
174 sIniDirectory =
"Platform/IniFiles/";
175 ROS_WARN(
"IniDirectory not found on Parameter-Server, using default value: %s", sIniDirectory.c_str());
179 if(m_bPubEffort)
ROS_INFO(
"You have choosen to publish effort of motors, that charges capacity of CAN");
183 iniFile.
SetFileName(sIniDirectory +
"Platform.ini",
"PltfHardwareCoB3.h");
186 iniFile.
GetKeyInt(
"Config",
"NumberOfMotors", &m_iNumMotors,
true);
187 iniFile.
GetKeyInt(
"Config",
"NumberOfWheels", &m_iNumDrives,
true);
188 if(m_iNumMotors < 2 || m_iNumMotors > 8) {
194 bl_caster_pub = n.
advertise<std_msgs::Float64>(
"/base_bl_caster_r_wheel_controller/command", 1);
195 br_caster_pub = n.
advertise<std_msgs::Float64>(
"/base_br_caster_r_wheel_controller/command", 1);
196 fl_caster_pub = n.
advertise<std_msgs::Float64>(
"/base_fl_caster_r_wheel_controller/command", 1);
197 fr_caster_pub = n.
advertise<std_msgs::Float64>(
"/base_fr_caster_r_wheel_controller/command", 1);
198 bl_steer_pub = n.
advertise<std_msgs::Float64>(
"/base_bl_caster_rotation_controller/command", 1);
199 br_steer_pub = n.
advertise<std_msgs::Float64>(
"/base_br_caster_rotation_controller/command", 1);
200 fl_steer_pub = n.
advertise<std_msgs::Float64>(
"/base_fl_caster_rotation_controller/command", 1);
201 fr_steer_pub = n.
advertise<std_msgs::Float64>(
"/base_fr_caster_rotation_controller/command", 1);
203 topicSub_GazeboJointStates = n.
subscribe(
"/joint_states", 1, &NodeClass::gazebo_joint_states_Callback,
this);
205 m_gazeboPos.resize(m_iNumMotors);
206 m_gazeboVel.resize(m_iNumMotors);
208 topicPub_JointState = n.
advertise<sensor_msgs::JointState>(
"/joint_states", 1);
214 topicPub_ControllerState = n.
advertise<control_msgs::JointTrajectoryControllerState>(
"state", 1);
215 topicPub_DiagnosticGlobal_ = n.
advertise<diagnostic_msgs::DiagnosticArray>(
"/diagnostics", 1);
217 topicPub_Diagnostic = n.
advertise<diagnostic_msgs::DiagnosticStatus>(
"diagnostic", 1);
225 m_bReadoutElmo =
false;
237 m_bisInitialized =
false;
256 ROS_DEBUG(
"Topic Callback joint_command");
258 if(m_bisInitialized ==
true)
260 ROS_DEBUG(
"Topic Callback joint_command - Sending Commands to drives (initialized)");
261 sensor_msgs::JointState JointStateCmd;
262 JointStateCmd.position.resize(m_iNumMotors);
263 JointStateCmd.velocity.resize(m_iNumMotors);
264 JointStateCmd.effort.resize(m_iNumMotors);
266 for(
unsigned int i = 0; i < msg->joint_names.size(); i++)
273 if(msg->joint_names[i] ==
"fl_caster_r_wheel_joint")
275 JointStateCmd.position[0] = msg->desired.positions[i];
276 JointStateCmd.velocity[0] = msg->desired.velocities[i];
279 else if(m_iNumDrives>=2 && msg->joint_names[i] ==
"bl_caster_r_wheel_joint")
281 JointStateCmd.position[2] = msg->desired.positions[i];
282 JointStateCmd.velocity[2] = msg->desired.velocities[i];
285 else if(m_iNumDrives>=3 && msg->joint_names[i] ==
"br_caster_r_wheel_joint")
287 JointStateCmd.position[4] = msg->desired.positions[i];
288 JointStateCmd.velocity[4] = msg->desired.velocities[i];
291 else if(m_iNumDrives>=4 && msg->joint_names[i] ==
"fr_caster_r_wheel_joint")
293 JointStateCmd.position[6] = msg->desired.positions[i];
294 JointStateCmd.velocity[6] = msg->desired.velocities[i];
298 else if(msg->joint_names[i] ==
"fl_caster_rotation_joint")
300 JointStateCmd.position[1] = msg->desired.positions[i];
301 JointStateCmd.velocity[1] = msg->desired.velocities[i];
304 else if(m_iNumDrives>=2 && msg->joint_names[i] ==
"bl_caster_rotation_joint")
306 JointStateCmd.position[3] = msg->desired.positions[i];
307 JointStateCmd.velocity[3] = msg->desired.velocities[i];
310 else if(m_iNumDrives>=3 && msg->joint_names[i] ==
"br_caster_rotation_joint")
312 JointStateCmd.position[5] = msg->desired.positions[i];
313 JointStateCmd.velocity[5] = msg->desired.velocities[i];
316 else if(m_iNumDrives>=4 && msg->joint_names[i] ==
"fr_caster_rotation_joint")
318 JointStateCmd.position[7] = msg->desired.positions[i];
319 JointStateCmd.velocity[7] = msg->desired.velocities[i];
324 ROS_ERROR(
"Unkown joint name %s", (msg->joint_names[i]).c_str());
335 if( i == 1 || i == 3 || i == 5 || i == 7)
360 ROS_DEBUG(
"Send velocity data to gazebo");
361 std_msgs::Float64 fl;
362 fl.data = JointStateCmd.velocity[i];
363 if(msg->joint_names[i] ==
"fl_caster_r_wheel_joint")
365 if(msg->joint_names[i] ==
"fr_caster_r_wheel_joint")
367 if(msg->joint_names[i] ==
"bl_caster_r_wheel_joint")
369 if(msg->joint_names[i] ==
"br_caster_r_wheel_joint")
372 if(msg->joint_names[i] ==
"fl_caster_rotation_joint")
374 if(msg->joint_names[i] ==
"fr_caster_rotation_joint")
376 if(msg->joint_names[i] ==
"bl_caster_rotation_joint")
378 if(msg->joint_names[i] ==
"br_caster_rotation_joint")
380 ROS_DEBUG(
"Successfully sent velicities to gazebo");
382 ROS_DEBUG(
"Send velocity data to drives");
384 ROS_DEBUG(
"Successfully sent velicities to drives");
403 std_srvs::Trigger::Response &res )
406 if(m_bisInitialized ==
false)
418 res.message =
"initialization of base failed";
424 ROS_WARN(
"...base already initialized...");
426 res.message =
"platform already initialized";
432 cob_base_drive_chain::ElmoRecorderConfig::Response &res ){
439 res.success = m_CanCtrlPltf->
ElmoRecordings(0, req.recordinggap,
"");
441 res.message =
"Successfully configured all motors for instant record";
448 cob_base_drive_chain::ElmoRecorderReadout::Response &res ){
449 if(m_bisInitialized) {
454 res.success = m_CanCtrlPltf->
ElmoRecordings(1, req.subindex, req.fileprefix);
456 if(res.success == 0) {
457 res.message =
"Successfully requested reading out of Recorded data";
458 m_bReadoutElmo =
true;
459 ROS_WARN(
"CPU consuming evalCanBuffer used for ElmoReadout activated");
460 }
else if(res.success == 1) res.message =
"Recorder hasn't been configured well yet";
461 else if(res.success == 2) res.message =
"A previous transmission is still in progress";
471 std_srvs::Trigger::Response &res )
479 res.success = m_CanCtrlPltf->
resetPltf();
484 res.message =
"reset of base failed";
490 ROS_WARN(
"Base not yet initialized.");
492 res.message =
"Base not yet initialized.";
499 std_srvs::Trigger::Response &res )
510 ROS_INFO(
"Shutdown of Drives FAILED");
521 std::vector<double> vdAngGearRad, vdVelGearRad, vdEffortGearNM;
524 vdAngGearRad.resize(m_iNumMotors, 0);
525 vdVelGearRad.resize(m_iNumMotors, 0);
526 vdEffortGearNM.resize(m_iNumMotors, 0);
529 sensor_msgs::JointState jointstate;
530 diagnostic_msgs::DiagnosticStatus diagnostics;
531 control_msgs::JointTrajectoryControllerState controller_state;
535 jointstate.header.stamp = m_gazeboStamp;
536 controller_state.header.stamp = m_gazeboStamp;
539 controller_state.header.stamp = jointstate.header.stamp;
544 jointstate.position.assign(m_iNumMotors, 0.0);
545 jointstate.velocity.assign(m_iNumMotors, 0.0);
546 jointstate.effort.assign(m_iNumMotors, 0.0);
548 if(m_bisInitialized ==
false)
559 jointstate.position[i] = 0.0;
560 jointstate.velocity[i] = 0.0;
561 jointstate.effort[i] = 0.0;
563 jointstate.name.push_back(
"fl_caster_r_wheel_joint");
564 jointstate.name.push_back(
"fl_caster_rotation_joint");
565 jointstate.name.push_back(
"bl_caster_r_wheel_joint");
566 jointstate.name.push_back(
"bl_caster_rotation_joint");
567 jointstate.name.push_back(
"br_caster_r_wheel_joint");
568 jointstate.name.push_back(
"br_caster_rotation_joint");
569 jointstate.name.push_back(
"fr_caster_r_wheel_joint");
570 jointstate.name.push_back(
"fr_caster_rotation_joint");
571 jointstate.name.resize(m_iNumMotors);
583 ROS_DEBUG(
"Successfully read CAN-Buffer");
590 vdAngGearRad[i] = m_gazeboPos[i];
591 vdVelGearRad[i] = m_gazeboVel[i];
609 if( i == 1 || i == 3 || i == 5 || i == 7)
621 jointstate.position[i] = vdAngGearRad[i];
622 jointstate.velocity[i] = vdVelGearRad[i];
623 jointstate.effort[i] = vdEffortGearNM[i];
626 jointstate.name.push_back(
"fl_caster_r_wheel_joint");
627 jointstate.name.push_back(
"fl_caster_rotation_joint");
628 jointstate.name.push_back(
"bl_caster_r_wheel_joint");
629 jointstate.name.push_back(
"bl_caster_rotation_joint");
630 jointstate.name.push_back(
"br_caster_r_wheel_joint");
631 jointstate.name.push_back(
"br_caster_rotation_joint");
632 jointstate.name.push_back(
"fr_caster_r_wheel_joint");
633 jointstate.name.push_back(
"fr_caster_rotation_joint");
634 jointstate.name.resize(m_iNumMotors);
638 controller_state.joint_names = jointstate.name;
639 controller_state.actual.positions = jointstate.position;
640 controller_state.actual.velocities = jointstate.velocity;
646 topicPub_JointState.
publish(jointstate);
648 topicPub_ControllerState.
publish(controller_state);
650 ROS_DEBUG(
"published new drive-chain configuration (JointState message)");
666 diagnostics.level = 2;
667 diagnostics.name =
"drive-chain can node";
668 diagnostics.message =
"one or more drives are in Error mode";
672 if (m_bisInitialized)
674 diagnostics.level = 0;
675 diagnostics.name =
"drive-chain can node";
676 diagnostics.message =
"drives operating normal";
680 diagnostics.level = 1;
681 diagnostics.name =
"drive-chain can node";
682 diagnostics.message =
"drives are initializing";
687 topicPub_Diagnostic.
publish(diagnostics);
688 ROS_DEBUG(
"published new drive-chain configuration (JointState message)");
697 diagnostic_msgs::DiagnosticArray diagnostics_gl;
699 diagnostics_gl.status.resize(1);
704 if(m_bisInitialized && m_CanCtrlPltf->
isPltfError())
708 diagnostics_gl.status[0].level = 2;
710 diagnostics_gl.status[0].message =
"Base not initialized or in error";
714 if (m_bisInitialized)
716 diagnostics_gl.status[0].level = 0;
718 diagnostics_gl.status[0].message =
"base_drive_chain initialized and running";
722 diagnostics_gl.status[0].level = 1;
724 diagnostics_gl.status[0].message =
"base_drive_chain not initialized";
728 topicPub_DiagnosticGlobal_.
publish(diagnostics_gl);
735 void gazebo_joint_states_Callback(
const sensor_msgs::JointState::ConstPtr& msg) {
736 for (
unsigned int i=0; i<msg->name.size(); i++) {
738 if(msg->name[i] ==
"fl_caster_r_wheel_joint") {
739 m_gazeboPos[0] = msg->position[i];
740 m_gazeboVel[0] = msg->velocity[i];
741 m_gazeboStamp = msg->header.stamp;
743 else if(msg->name[i] ==
"bl_caster_r_wheel_joint") {
744 m_gazeboPos[2] = msg->position[i];
745 m_gazeboVel[2] = msg->velocity[i];
747 else if(msg->name[i] ==
"br_caster_r_wheel_joint") {
748 m_gazeboPos[4] = msg->position[i];
749 m_gazeboVel[4] = msg->velocity[i];
751 else if(msg->name[i] ==
"fr_caster_r_wheel_joint") {
752 m_gazeboPos[6] = msg->position[i];
753 m_gazeboVel[6] = msg->velocity[i];
757 else if(msg->name[i] ==
"fl_caster_rotation_joint") {
758 m_gazeboPos[1] = msg->position[i];
759 m_gazeboVel[1] = msg->velocity[i];
761 else if(msg->name[i] ==
"bl_caster_rotation_joint") {
762 m_gazeboPos[3] = msg->position[i];
763 m_gazeboVel[3] = msg->velocity[i];
765 else if(msg->name[i] ==
"br_caster_rotation_joint") {
766 m_gazeboPos[5] = msg->position[i];
767 m_gazeboVel[5] = msg->velocity[i];
769 else if(msg->name[i] ==
"fr_caster_rotation_joint") {
770 m_gazeboPos[7] = msg->position[i];
771 m_gazeboVel[7] = msg->velocity[i];
782 int main(
int argc,
char** argv)
785 ros::init(argc, argv,
"base_drive_chain");
792 while(nodeClass.
n.
ok())
804 ROS_INFO(
"CPU consuming evalCanBuffer used for ElmoReadout deactivated");
822 ROS_INFO(
"Initializing Base Drive Chain");
867 ROS_INFO(
"Initializing CanCtrlItf");
CanCtrlPltfCOb3 * m_CanCtrlPltf
bool srvCallback_Recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
std::string sIniDirectory
int setVelGearRadS(int iCanIdent, double dVelGearRadS)
void publish_globalDiagnostics(const ros::TimerEvent &event)
ros::Publisher topicPub_JointState
ros::Publisher topicPub_ControllerState
void publish(const boost::shared_ptr< M > &message) const
ros::ServiceServer srvServer_ElmoRecorderConfig
bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
ros::Publisher topicPub_DiagnosticGlobal_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceServer srvServer_Init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
ros::Publisher topicPub_Diagnostic
ros::ServiceServer srvServer_Shutdown
ros::ServiceServer srvServer_SetMotionType
void requestMotorTorque()
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static void normalizePi(double &angle)
bool publish_JointStates()
ros::Subscriber topicSub_JointStateCmd
int main(int argc, char **argv)
void getMotorTorque(int iCanIdent, double *pdTorqueNm)
int GetKeyInt(const char *pSect, const char *pKey, int *pValue, bool bWarnIfNotfound=true)
void topicCallback_JointStateCmd(const control_msgs::JointTrajectoryControllerState::ConstPtr &msg)
bool srvCallback_Shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::Timer glDiagnostics_timer
bool srvCallback_ElmoRecorderConfig(cob_base_drive_chain::ElmoRecorderConfig::Request &req, cob_base_drive_chain::ElmoRecorderConfig::Response &res)
int getGearPosVelRadS(int iCanIdent, double *pdAngleGearRad, double *pdVelGearRadS)
int GetKeyDouble(const char *pSect, const char *pKey, double *pValue, bool bWarnIfNotfound=true)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int SetFileName(std::string fileName, std::string strIniFileUsedBy="", bool bCreate=false)
bool srvCallback_ElmoRecorderReadout(cob_base_drive_chain::ElmoRecorderReadout::Request &req, cob_base_drive_chain::ElmoRecorderReadout::Response &res)
double dMaxSteerRateRadpS
ros::ServiceServer srvServer_ElmoRecorderReadout
std::vector< double > vdWheelNtrlPosRad
static double convDegToRad(const double &dAngDeg)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
double dMaxDriveRateRadpS
ROSCPP_DECL void spinOnce()
int ElmoRecordings(int iFlag, int iParam, std::string sString)
ros::ServiceServer srvServer_Recover