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

// 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);
}

// 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
//.........这里部分代码省略.........