catkin_create_pkg odom_tf_package std_msgs rospy roscpp sensor_msgs tf nav_msgs

 gedit odom_tf_node.cpp

复制代码#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>int main(int argc, char** argv)
{ros::init(argc, argv, "odometry_publisher");ros::NodeHandle n;ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);tf::TransformBroadcaster odom_broadcaster;double x = 0.0;double y = 0.0;double th = 0.0;double vx = 0.1;double vy = -0.1;double vth = 0.1;ros::Time current_time, last_time;current_time = ros::Time::now();last_time = ros::Time::now();ros::Rate r(1.0);while(n.ok()){ros::spinOnce();               // check for incoming messagescurrent_time = ros::Time::now();//compute odometry in a typical way given the velocities of the robotdouble dt = (current_time - last_time).toSec();double delta_x = (vx * cos(th) - vy * sin(th)) * dt;double delta_y = (vx * sin(th) + vy * cos(th)) * dt;double delta_th = vth * dt;x += delta_x;y += delta_y;th += delta_th;//since all odometry is 6DOF we'll need a quaternion created from yawgeometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);//first, we'll publish the transform over tfgeometry_msgs::TransformStamped odom_trans;odom_trans.header.stamp = current_time;odom_trans.header.frame_id = "odom";odom_trans.child_frame_id = "base_link";odom_trans.transform.translation.x = x;odom_trans.transform.translation.y = y;odom_trans.transform.translation.z = 0.0;odom_trans.transform.rotation = odom_quat;//send the transformodom_broadcaster.sendTransform(odom_trans);//next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;odom.header.stamp = current_time;odom.header.frame_id = "odom";//set the positionodom.pose.pose.position.x = x;odom.pose.pose.position.y = y;odom.pose.pose.position.z = 0.0;odom.pose.pose.orientation = odom_quat;//set the velocityodom.child_frame_id = "base_link";odom.twist.twist.linear.x = vx;odom.twist.twist.linear.y = vy;odom.twist.twist.angular.z = vth;//publish the messageodom_pub.publish(odom);last_time = current_time;r.sleep();}
}

    #include <tf/transform_broadcaster.h>#include <nav_msgs/Odometry.h>

      ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);  tf::TransformBroadcaster odom_broadcaster;

      double x = 0.0;double y = 0.0; double th = 0.0;

      double vx = 0.1;double vy = -0.1; double vth = 0.1;

  ros::Rate r(1.0);

        //compute odometry in a typical way given the velocities of the robotdouble dt = (current_time - last_time).toSec();double delta_x = (vx * cos(th) - vy * sin(th)) * dt; double delta_y = (vx * sin(th) + vy * cos(th)) * dt; double delta_th = vth * dt; x += delta_x; y += delta_y; th += delta_th;

       //since all odometry is 6DOF we'll need a quaternion created from yawgeometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        //first, we'll publish the transform over tfgeometry_msgs::TransformStamped odom_trans;odom_trans.header.stamp = current_time;odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;odom_trans.transform.translation.y = y;odom_trans.transform.translation.z = 0.0;odom_trans.transform.rotation = odom_quat;

        //next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;odom.header.stamp = current_time;

        //set the positionodom.pose.pose.position.x = x;odom.pose.pose.position.y = y;odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; //set the velocity odom.child_frame_id = "base_link"; odom.twist.twist.linear.x = vx; odom.twist.twist.linear.y = vy; odom.twist.twist.angular.z = vth;

add_executable(odom_tf_node src/odom_tf_node.cpp)target_link_libraries(odom_tf_node ${catkin_LIBRARIES})

catkin_make 

    #Standard metadata for higher-level flow data types#sequence ID: consecutively increasing ID uint32 seq    #Two-integer timestamp that is expressed as:# * stamp.secs: seconds (stamp_secs) since epoch # * stamp.nsecs: nanoseconds since stamp_secs # time-handling sugar is provided by the client library time stamp #Frame this data is associated with # 0: no frame # 1: global frame string frame_id

    #This message holds a collection of 3d points, plus optional additional information about each point.   #Each Point32 should be interpreted as a 3d point in the frame given in the header          Header header     geometry_msgs/Point32[] points #Array of 3d points ChannelFloat32[] channels #Each channel should have the same number of elements as points array, and the data in each channel should correspond 1:1 with each point

gedit point_kinect_node.cpp

#include "ros/ros.h"
#include <sensor_msgs/PointCloud.h>int main(int argc, char** argv) { ros::init(argc, argv, "point_cloud_publisher"); ros::NodeHandle n; ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50); unsigned int num_points = 100; int count = 0; ros::Rate r(1.0); while(n.ok()) { sensor_msgs::PointCloud cloud; cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = "sensor_frame"; cloud.points.resize(num_points); //we'll also add an intensity channel to the cloud cloud.channels.resize(1); cloud.channels[0].name = "intensities"; cloud.channels[0].values.resize(num_points); //generate some fake data for our point cloud for(unsigned int i = 0; i < num_points; ++i) { cloud.points[i].x = 1 + count; cloud.points[i].y = 2 + count; cloud.points[i].z = 3 + count; cloud.channels[0].values[i] = 100 + count; } cloud_pub.publish(cloud); ++count; r.sleep(); } }

#include <sensor_msgs/PointCloud.h>

    ros::Publisher cloud_pub = n.advertise<sensor_msgs::PointCloud>("cloud", 50);

        sensor_msgs::PointCloud cloud;cloud.header.stamp = ros::Time::now();cloud.header.frame_id = "sensor_frame";

    cloud.points.resize(num_points);

        //we'll also add an intensity channel to the cloudcloud.channels.resize(1);cloud.channels[0].name = "intensities"; cloud.channels[0].values.resize(num_points);

        //generate some fake data for our point cloudfor(unsigned int i = 0; i < num_points; ++i){ cloud.points[i].x = 1 + count; cloud.points[i].y = 2 + count; cloud.points[i].z = 3 + count; cloud.channels[0].values[i] = 100 + count;  

   cloud_pub.publish(cloud);

add_executable(point_kinect_node src/point_kinect_node.cpp)target_link_libraries(point_kinect_node ${catkin_LIBRARIES})

catkin_make

roscorerosrun odom_tf_package odom_tf_noderosrun odom_tf_package point_kinect_node rviz

rostopic echo /cloud