当前位置: 首页>>代码示例>>C++>>正文


C++ UndercarriageCtrlGeom::GetActualPltfVelocity方法代码示例

本文整理汇总了C++中UndercarriageCtrlGeom::GetActualPltfVelocity方法的典型用法代码示例。如果您正苦于以下问题:C++ UndercarriageCtrlGeom::GetActualPltfVelocity方法的具体用法?C++ UndercarriageCtrlGeom::GetActualPltfVelocity怎么用?C++ UndercarriageCtrlGeom::GetActualPltfVelocity使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在UndercarriageCtrlGeom的用法示例。


在下文中一共展示了UndercarriageCtrlGeom::GetActualPltfVelocity方法的2个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: UpdateOdometry

// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
	double vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
	double dummy1, dummy2;
	double dt;
	ros::Time current_time;

	// if drive chain already initialized process joint data
	if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
	{
		// Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
		// !Careful! Controller internally calculates with mm instead of m
		// ToDo: change internal calculation to SI-Units
		// ToDo: last values are not used anymore --> remove from interface
		ucar_ctrl_.GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
									vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);

		// convert variables to SI-Units
		vel_x_rob_ms = vel_x_rob_ms/1000.0;
		vel_y_rob_ms = vel_y_rob_ms/1000.0;
		delta_x_rob_m = delta_x_rob_m/1000.0;
		delta_y_rob_m = delta_y_rob_m/1000.0;
	}
	else
	{
		// otherwise set data (velocity and pose-delta) to zero
		vel_x_rob_ms = 0.0;
		vel_y_rob_ms = 0.0;
		delta_x_rob_m = 0.0;
		delta_y_rob_m = 0.0;
	}

	// calc odometry (from startup)
	// get time since last odometry-measurement
	current_time = ros::Time::now();
	dt = current_time.toSec() - last_time_.toSec();
	last_time_ = current_time;
	// calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom
	x_rob_m_ = x_rob_m_ + (vel_x_rob_ms * cos(theta_rob_rad_) - vel_y_rob_ms * sin(theta_rob_rad_)) * dt;
	y_rob_m_ = y_rob_m_ + (vel_x_rob_ms * sin(theta_rob_rad_) + vel_y_rob_ms * cos(theta_rob_rad_)) * dt;
	theta_rob_rad_ = rot_rob_rads * dt;

	// format data for compatibility with tf-package and standard odometry msg
	// generate quaternion for rotation
	geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);

	// compose and publish transform for tf package
	geometry_msgs::TransformStamped odom_tf;
	// compose header
	odom_tf.header.stamp = current_time;
	odom_tf.header.frame_id = "/odom";
	odom_tf.child_frame_id = "/base_footprint";
	// compose data container
	odom_tf.transform.translation.x = x_rob_m_;
	odom_tf.transform.translation.y = y_rob_m_;
	odom_tf.transform.translation.z = 0.0;
	odom_tf.transform.rotation = odom_quat;

    // compose and publish odometry message as topic
    nav_msgs::Odometry odom_top;
	// compose header
    odom_top.header.stamp = current_time;
    odom_top.header.frame_id = "/odom";
    odom_top.child_frame_id = "/base_footprint";
    // compose pose of robot
    odom_top.pose.pose.position.x = x_rob_m_;
    odom_top.pose.pose.position.y = y_rob_m_;
    odom_top.pose.pose.position.z = 0.0;
    odom_top.pose.pose.orientation = odom_quat;
    // compose twist of robot
    odom_top.twist.twist.linear.x = vel_x_rob_ms;
    odom_top.twist.twist.linear.y = vel_y_rob_ms;
    odom_top.twist.twist.linear.z = 0.0;
    odom_top.twist.twist.angular.x = 0.0;
    odom_top.twist.twist.angular.y = 0.0;
    odom_top.twist.twist.angular.z = rot_rob_rads;

	// publish data
	// publish the transform
	tf_broadcast_odometry_.sendTransform(odom_tf);
	// publish odometry msg
	topic_pub_odometry_.publish(odom_top);
}
开发者ID:clmi-bas,项目名称:care-o-bot,代码行数:85,代码来源:cob_undercarriage_ctrl.cpp

示例2: UpdateOdometry

// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
    double vel_x_rob_ms, vel_y_rob_ms, vel_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
    double dummy1, dummy2;
    double dt;
    ros::Time current_time;

    // if drive chain already initialized process joint data
    //if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
    if (is_initialized_bool_)
    {
        // Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
        // !Careful! Controller internally calculates with mm instead of m
        // ToDo: change internal calculation to SI-Units
        // ToDo: last values are not used anymore --> remove from interface
        ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
                                          vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);

        // convert variables to SI-Units
        vel_x_rob_ms = vel_x_rob_ms/1000.0;
        vel_y_rob_ms = vel_y_rob_ms/1000.0;
        delta_x_rob_m = delta_x_rob_m/1000.0;
        delta_y_rob_m = delta_y_rob_m/1000.0;

        ROS_DEBUG("Odmonetry delta is: x=%f, y=%f, th=%f", delta_x_rob_m, delta_y_rob_m, rot_rob_rads);
    }
    else
    {
        // otherwise set data (velocity and pose-delta) to zero
        vel_x_rob_ms = 0.0;
        vel_y_rob_ms = 0.0;
        delta_x_rob_m = 0.0;
        delta_y_rob_m = 0.0;
    }

    // calc odometry (from startup)
    // get time since last odometry-measurement
    current_time = ros::Time::now();
    dt = current_time.toSec() - last_time_.toSec();
    last_time_ = current_time;
    vel_rob_ms = sqrt(vel_x_rob_ms*vel_x_rob_ms + vel_y_rob_ms*vel_y_rob_ms);

    // calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration
    x_rob_m_ = x_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * cos(theta_rob_rad_) - (vel_y_rob_ms+vel_y_rob_last_)/2.0 * sin(theta_rob_rad_)) * dt;
    y_rob_m_ = y_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * sin(theta_rob_rad_) + (vel_y_rob_ms+vel_y_rob_last_)/2.0 * cos(theta_rob_rad_)) * dt;
    theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt;
    //theta_rob_rad_ = theta_rob_rad_ + (rot_rob_rads+vel_theta_rob_last_)/2.0 * dt;

    vel_x_rob_last_ = vel_x_rob_ms;
    vel_y_rob_last_ = vel_y_rob_ms;
    vel_theta_rob_last_ = rot_rob_rads;


    // format data for compatibility with tf-package and standard odometry msg
    // generate quaternion for rotation
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);

    // compose and publish transform for tf package
    geometry_msgs::TransformStamped odom_tf;
    // compose header
    odom_tf.header.stamp = joint_state_odom_stamp_;
    odom_tf.header.frame_id = "/wheelodom";
    odom_tf.child_frame_id = "/base_footprint";
    // compose data container
    odom_tf.transform.translation.x = x_rob_m_;
    odom_tf.transform.translation.y = y_rob_m_;
    odom_tf.transform.translation.z = 0.0;
    odom_tf.transform.rotation = odom_quat;

    // compose and publish odometry message as topic
    nav_msgs::Odometry odom_top;
    // compose header
    odom_top.header.stamp = joint_state_odom_stamp_;
    odom_top.header.frame_id = "/wheelodom";
    odom_top.child_frame_id = "/base_footprint";
    // compose pose of robot
    odom_top.pose.pose.position.x = x_rob_m_;
    odom_top.pose.pose.position.y = y_rob_m_;
    odom_top.pose.pose.position.z = 0.0;
    odom_top.pose.pose.orientation = odom_quat;
    for(int i = 0; i < 6; i++)
        odom_top.pose.covariance[i*6+i] = 0.1;

    // compose twist of robot
    odom_top.twist.twist.linear.x = vel_x_rob_ms;
    odom_top.twist.twist.linear.y = vel_y_rob_ms;
    odom_top.twist.twist.linear.z = 0.0;
    odom_top.twist.twist.angular.x = 0.0;
    odom_top.twist.twist.angular.y = 0.0;
    odom_top.twist.twist.angular.z = rot_rob_rads;
    for(int i = 0; i < 6; i++)
        odom_top.twist.covariance[6*i+i] = 0.1;

    if (fabs(vel_x_rob_ms) > 0.005 or fabs(vel_y_rob_ms) > 0.005 or fabs(rot_rob_rads) > 0.005)
    {
        is_moving_ = true;
    }
    else
//.........这里部分代码省略.........
开发者ID:RomanRobotnik,项目名称:cob_driver,代码行数:101,代码来源:cob_undercarriage_ctrl.cpp


注:本文中的UndercarriageCtrlGeom::GetActualPltfVelocity方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。