Great tutorial from github

it 's about position control of the turtlesim 
the source code page is from

https://github.com/utari/UTARI_ROSTutorials/wiki/TurtlesimPositionController-Tutorial

Desired Location

This will be a new topic created by ourselves. to simply follow conventions we will use the same beginning of the other topics "/turtle1/", this is sometimes referred to as the namespace.
Chosen topic name: "/turtle1/PositionCommand"
We will have to choose an appropriate msg type. We could use turtlesim/Pose again but as it has unneeded velocity information and to learn about other types we will look through the ROS wiki. Looking through the wiki pages with ros msgs...
http://wiki.ros.org/std_msgs
http://wiki.ros.org/geometry_msgs
We can see the topic type Pose2D under geometry_msgs. This contains X, Y, and Theta information only. The goal of this controller is simply position and not orientation (Theta) but we can use this type for X and Y. A third or higher order controller could be used to have the turtle approach the point from the correct direction.
This msg will also have the c++ header associated with it #include - "geometry_msgs/Pose2D.h".
We will have to add geometry_msgs to our package dependency's for ROS to find this header. In the package root open package.xml and make sure it looks as follows, where CREATOR will likely be your Ubuntu user name.

<package>
  <description brief="TurtlesimPositionController_pkg">

     TurtlesimPositionController_pkg

  </description>
  <author>CREATOR</author>
  <license>BSD</license>
  <review status="unreviewed" notes=""/>
  <url>http://ros.org/wiki/TurtlesimPositionController_pkg</url>
  
  <depend package="turtlesim"/>
  <depend package="rospy"/>
  <depend package="roscpp"/>
  <depend package="geometry_msgs"/>
</package>

It has been noted for an unknown reason that the package and node may build correctly if you skip adding geometry_msgs to the manifest. This is not the case for most msgs and headers.

// i just forget how this xml file work would it really need the geometry_msgs   o?

Adding the Publishers and Subscribers

We can now add the commands to make our node connect to the found ROS topics. These additions are the #includes for the headers, creating the ROS subscribers and publishers, and callbacks for the subscribers. ros::spin will be added for now to check for incoming messages.

#include "ros/ros.h"#include "geometry_msgs/Pose2D.h" // to get desired position command#include "turtlesim/Pose.h" // to read current position#include "geometry_msgs/Twist.h" // to send velocity command// Function declarationsvoid ComPoseCallback(const geometry_msgs::Pose2D::ConstPtr& msg);void CurPoseCallback(const turtlesim::Pose::ConstPtr& msg);int main(int argc, char **argv){ros::init(argc, argv, "TurtlesimPositionController_pubsub"); // connect to roscoreros::NodeHandle n; // node object// register sub to get desired position/pose commandsros::Subscriber ComPose_sub = n.subscribe("/turtle1/PositionCommand", 5, ComPoseCallback);// register sub to get current position/poseros::Subscriber CurPose_sub = n.subscribe("/turtle1/pose", 5, CurPoseCallback);// register pub to send twist velocity (cmd_vel)ros::Publisher Twist_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);ros::spin();}// call back to send new desired Pose msgsvoid ComPoseCallback(const geometry_msgs::Pose2D::ConstPtr& msg){ROS_INFO("Received Command msg");return;}// call back to send new current Pose msgsvoid CurPoseCallback(const turtlesim::Pose::ConstPtr& msg){ROS_INFO("Received Pose msg");return;}

// this remains undone .

There are many places you could place your processing code, in main, or in your callbacks. For simplicity we will use main as our processing area. Assuming we want the controller to run constantly we will place the code in a while loop, change ros::spin to ros::spinOnce, and use ros::Rate to control the loop frequency.

Getting Data

We now are receiving our input data but throwing it away with every callback. The simplest way to hold onto that data to be processed through main is a global variable holder. We will make variables of our message data types and copy them in the callback.
// 
#include "ros/ros.h"#include "geometry_msgs/Pose2D.h" // to get desired position command#include "turtlesim/Pose.h" // to read current position#include "geometry_msgs/Twist.h" // to send velocity command// Function declarationsvoid ComPoseCallback(const geometry_msgs::Pose2D::ConstPtr& msg);void CurPoseCallback(const turtlesim::Pose::ConstPtr& msg);// Global variablesbool STOP = true; // to hold stop flag, wait till first command giventurtlesim::Pose CurPose; // to hold current posegeometry_msgs::Pose2D DesPose; // variable to hold desired poseint main(int argc, char **argv){ros::init(argc, argv, "TurtlesimPositionController_pubsub"); // connect to roscoreros::NodeHandle n; // node object// register sub to get desired position/pose commandsros::Subscriber ComPose_sub = n.subscribe("/turtle1/PositionCommand", 5, ComPoseCallback);// register sub to get current position/poseros::Subscriber CurPose_sub = n.subscribe("/turtle1/pose", 5, CurPoseCallback);// register pub to send twist velocity (cmd_vel)ros::Publisher Twist_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 100);ros::Rate loop_rate(10); // freq to run loops in (10 Hz)ROS_INFO("Ready to send position commands"); // let user know we are ready and goodwhile (ros::ok() && n.ok() ) // while ros and the node are ok{ros::spinOnce();if (STOP == false) // and no stop command{printf("Processing...\n");}else{printf("Waiting...\n");}loop_rate.sleep(); // sleep to maintain loop rate}}// call back to send new desired Pose msgsvoid ComPoseCallback(const geometry_msgs::Pose2D::ConstPtr& msg){STOP = false; // start loopDesPose.x = msg->x; // copy msg to varible to use elsewhereDesPose.y = msg->y;return;}// call back to send new current Pose msgsvoid CurPoseCallback(const turtlesim::Pose::ConstPtr& msg){CurPose.x = msg->x;CurPose.y = msg->y;CurPose.theta = msg->theta; // copy msg to varible to use elsewherereturn;}

Data Processing

There are many places you could place your processing code, in main, or in your callbacks. For simplicity we will use main as our processing area. Assuming we want the controller to run constantly we will place the code in a while loop, change ros::spin to ros::spinOnce, and use ros::Rate to control the loop frequency.
// that really matters when the ros::spinOnce and ros::Rate to controll the frequency of the loop 

http://docs.ros.org/diamondback/api/erratic_teleop/html/keyboard_8cpp_source.html that ' the robot controll 

阅读更多
想对作者说点什么? 我来说一句

没有更多推荐了,返回首页