Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <ros/ros.h>
- #include <tf/transform_listener.h>
- #include <iostream>
- #include <nav_msgs/Odometry.h>
- using namespace std;
- /*#include <turtlesim/Velocity.h>
- #include <turtlesim/Spawn.h>*/
- // %Tag(CALLBACK)%
- void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg)
- {
- ROS_INFO("I heard: [%f]", msg->pose.pose.position.x);
- }
- int main(int argc, char** argv){
- ros::init(argc, argv, "my_tf_listener");
- ros::NodeHandle node;
- // ros::service::waitForService("odom");
- /* ros::ServiceClient add_turtle =
- node.serviceClient<turtlesim::Spawn>("spawn");
- turtlesim::Spawn srv;
- add_turtle.call(srv);
- ros::Publisher turtle_vel =
- node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
- */
- tf::TransformListener listener;
- ros::Subscriber sub = node.subscribe("odom", 1000, chatterCallback);
- ros::spin();
- ros::Rate rate(1.0);
- while (node.ok()){
- tf::StampedTransform transform;
- try{
- listener.lookupTransform("tf", "odom",
- ros::Time::now(), transform);
- }
- catch (tf::TransformException ex){
- ROS_ERROR("%s",ex.what());
- }
- cout << transform.getOrigin().y()<<endl;
- cout << transform.getOrigin().x()<<endl;
- // turtlesim::Velocity vel_msg;
- /* vel_msg.angular = 4 * atan2(transform.getOrigin().y(),
- transform.getOrigin().x());
- vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
- pow(transform.getOrigin().y(), 2));
- turtle_vel.publish(vel_msg);*/
- rate.sleep();
- }
- return 0;
- };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement