本文整理汇总了C++中UndercarriageCtrlGeom::SetDesiredPltfVelocity方法的典型用法代码示例。如果您正苦于以下问题:C++ UndercarriageCtrlGeom::SetDesiredPltfVelocity方法的具体用法?C++ UndercarriageCtrlGeom::SetDesiredPltfVelocity怎么用?C++ UndercarriageCtrlGeom::SetDesiredPltfVelocity使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类UndercarriageCtrlGeom
的用法示例。
在下文中一共展示了UndercarriageCtrlGeom::SetDesiredPltfVelocity方法的6个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: topicCallbackTwistCmd
// Listen for Pltf Cmds
void topicCallbackTwistCmd(const geometry_msgs::Twist::ConstPtr& msg)
{
double vx_cmd_mms, vy_cmd_mms, w_cmd_rads;
// controller expects velocities in mm/s, ROS works with SI-Units -> convert
// ToDo: rework Controller Class to work with SI-Units
vx_cmd_mms = msg->linear.x*1000.0;
vy_cmd_mms = msg->linear.y*1000.0;
w_cmd_rads = msg->angular.z;
// only process if controller is already initialized
if (is_initialized_bool_)
{
ROS_DEBUG("received new velocity command [cmdVelX=%3.5f,cmdVelY=%3.5f,cmdVelTh=%3.5f]",
msg->linear.x, msg->linear.y, msg->angular.z);
// Set desired value for Plattform Velocity to UndercarriageCtrl (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity(vx_cmd_mms, vy_cmd_mms, w_cmd_rads, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
}
else
{
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
}
}
示例2: topicCallbackEMStop
// Listen for Emergency Stop
void topicCallbackEMStop(const cob_msgs::EmergencyStopState::ConstPtr& msg)
{
int EM_state;
EM_state = msg->emergency_state;
if (EM_state == msg->EMFREE)
{
// Reset EM flag in Ctrlr
if (is_initialized_bool_)
{
ucar_ctrl_.setEMStopActive(false);
// reset only done, when system initialized
// -> allows to stop ctrlr during init, reset and shutdown
}
}
else
{
ROS_DEBUG("Undercarriage Controller stopped due to EM-Stop");
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
// Set EM flag and stop Ctrlr
ucar_ctrl_.setEMStopActive(true);
}
}
示例3: topicCallbackDiagnostic
// Listens for status of underlying hardware (base drive chain)
void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
{
sensor_msgs::JointState joint_state_cmd;
// prepare joint_cmds for heartbeat (compose header)
joint_state_cmd.header.stamp = ros::Time::now();
//joint_state_cmd.header.frame_id = frame_id; //Where to get this id from?
// ToDo: configure over Config-File (number of motors) and Msg
// assign right size to JointState data containers
//joint_state_cmd.set_name_size(m_iNumMotors);
joint_state_cmd.position.resize(m_iNumJoints);
joint_state_cmd.velocity.resize(m_iNumJoints);
joint_state_cmd.effort.resize(m_iNumJoints);
// compose jointcmds
for(int i=0; i<m_iNumJoints; i++)
{
joint_state_cmd.position[i] = 0.0;
joint_state_cmd.velocity[i] = 0.0;
joint_state_cmd.effort[i] = 0.0;
}
// set status of underlying drive chain to member variable
drive_chain_diagnostic_ = msg->level;
// if controller is already started up ...
if (is_initialized_bool_)
{
// ... but underlying drive chain is not yet operating normal
if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
{
// halt controller
ROS_DEBUG("drive chain not availlable: halt Controller");
// Set EM flag to Ctrlr (resets internal states)
ucar_ctrl_->setEMStopActive(true);
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_->SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
ROS_DEBUG("Forced platform-velocity cmds to zero");
// if is not Initializing
if (drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
{
// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
topic_pub_joint_state_cmd_.publish(joint_state_cmd);
}
}
}
// ... while controller is not initialized send heartbeats to keep motors alive
else
{
// ... as soon as base drive chain is initialized
if(drive_chain_diagnostic_ != diagnostic_status_lookup_.WARN)
{
// publish zero-vel. jointcmds to avoid Watchdogs stopping ctrlr
topic_pub_joint_state_cmd_.publish(joint_state_cmd);
}
}
}
示例4: srvCallbackShutdown
// shutdown Controller
bool srvCallbackShutdown(cob_srvs::Switch::Request &req,
cob_srvs::Switch::Response &res )
{
if (is_initialized_bool_)
{
// stop controller
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// flag that controller is not running anymore
is_initialized_bool_ = false;
res.success = true;
}
else
{
// shutdown not possible, because pltf not running
ROS_ERROR("...platform not initialized...");
res.success = false;
res.errorMessage.data = "platform already or still down";
}
return true;
}
示例5: topicCallbackDiagnostic
// Listens for status of underlying hardware (base drive chain)
void topicCallbackDiagnostic(const diagnostic_msgs::DiagnosticStatus::ConstPtr& msg)
{
// set status of underlying drive chain to member variable
drive_chain_diagnostic_ = msg->level;
// if controller is already started up ...
if (is_initialized_bool_)
{
// ... but underlying drive chain is not yet operating normal
if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
{
// halt controller
ROS_DEBUG("drive chain not availlable: halt Controller");
// Set EM flag to Ctrlr (resets internal states)
ucar_ctrl_.setEMStopActive(true);
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
}
}
}
示例6: srvCallbackReset
// reset Controller Configuration
bool srvCallbackReset(cob_srvs::Switch::Request &req, cob_srvs::Switch::Response &res )
{
bool ctrlr_reset;
if (is_initialized_bool_)
{
// flag that Controller is not initialized
is_initialized_bool_ = false;
// first of all stop controller (similar to EMStop)
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
// Set EM flag and stop Ctrlr
ucar_ctrl_.setEMStopActive(true);
// now re-init controller configuration
ctrlr_reset = InitCtrl();
if (ctrlr_reset)
{
// restart controller
// Set desired value for Plattform Velocity to zero (setpoint setting)
ucar_ctrl_.SetDesiredPltfVelocity( 0.0, 0.0, 0.0, 0.0);
// ToDo: last value (0.0) is not used anymore --> remove from interface
// Set EM flag and stop Ctrlr
ucar_ctrl_.setEMStopActive(false);
// reset Time
last_time_ = ros::Time::now();
// and reset odometry
x_rob_m_ = 0.0;
y_rob_m_ = 0.0;
theta_rob_rad_ = 0.0;
// flag that controller is initialized
is_initialized_bool_ = true;
// set answer for service request
ROS_INFO("Undercarriage Controller resetted");
res.success = true;
}
else
{
// set answer for service request
ROS_INFO("Re-Init after Reset of Undercarriage Controller failed");
res.success = false;
res.errorMessage.data = "reinit after reset of undercarriage controller failed";
}
}
else
{
// Reset not possible, because Controller not yet set up
ROS_ERROR("... undercarriage controller not yet initialized, reset not possible ...");
// set answer for service request
res.success = false;
res.errorMessage.data = "undercarriage controller not yet initialized";
}
return true;
}