ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

subscribing to AMCL pose

asked 2016-11-14 06:56:40 -0500

adityakamath gravatar image

updated 2016-11-14 07:26:16 -0500

Hi, I am a ROS beginner and this might seem like a silly doubt but I haven't found the solution on the forums yet.

I have a robot which is localizing using AMCL. I wish to write a subscriber that can subscribe to the amcl_pose topic and read the geometry_msgs/PoseWithCovarianceStamped to be used somewhere else.

This is the code that I have for now but I am receiving a lot of errors but I am unclear on what is going wrong

#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
double poseAMCLx, poseAMCLy, poseAMCLa;
void poseAMCLCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msgAMCL)
{
    poseAMCLx = msgAMCL->pose.pose.position.x;
    poseAMCLy = msgAMCL->pose.pose.position.y;
    poseAMCLa = msgAMCL->pose.pose.orientation.w;   
    //ROS_INFO(msgAMCL);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "err_eval");
    ros::NodeHandle n;
    ros::Subscriber sub_amcl = n.subscribe<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", poseAMCLCallback);
    ros::Rate loop_rate(10);
    ros::spinOnce();

    int count = 0;
    while (ros::ok())
    {

        geometry_msgs::Pose error;
        error.position.x = poseAMCLx;
        error.position.y = poseAMCLy;
        error.orientation.w = poseAMCLa;
        pub.publish(error);

        ros::spinOnce();
        loop_rate.sleep();
        count=count+1;
    }

    return 0;
}

`

edit retag flag offensive close merge delete

Comments

maybe you could also post the errors you are receiving so we at least can guess what the actual problem is?

mgruhler gravatar image mgruhler  ( 2016-11-15 01:13:38 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2018-04-16 18:58:36 -0500

Karim Chamaa gravatar image

Change your subscriber statement to this: ros::Subscriber sub_amcl = n.subscribe("amcl_pose", 100, poseAMCLCallback);

edit flag offensive delete link more
0

answered 2020-04-27 05:09:04 -0500

ILYAS Meo gravatar image

!/usr/bin/env python

I did it like this in Python to get X,Y,Yaw

import rospy

import math from geometry_msgs.msg import PoseWithCovarianceStamped from tf.transformations import euler_from_quaternion, quaternion_from_euler

roll = pitch = yaw = 0.0

def get_rotation (msg): global roll, pitch, yaw orientation_q = msg.pose.pose.orientation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] (roll, pitch, yaw) = euler_from_quaternion (orientation_list)
print('X =',msg.pose.pose.position.x, 'Y =',msg.pose.pose.position.y, 'Yaw =',math.degrees(yaw))

rospy.init_node('my_quaternion_to_euler')

sub = rospy.Subscriber ('/amcl_pose', PoseWithCovarianceStamped, get_rotation) # geometry_msgs/PoseWithCovariance pose

r = rospy.Rate(1) while not rospy.is_shutdown(): quat = quaternion_from_euler (roll, pitch,yaw) # print quat r.sleep()

edit flag offensive delete link more

Question Tools

Stats

Asked: 2016-11-14 06:56:40 -0500

Seen: 4,507 times

Last updated: Nov 14 '16