Ask Your Question
0

subscribing to AMCL pose

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

adityakamath gravatar image

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

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 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
1

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

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 -0600

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

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

Seen: 2,783 times

Last updated: Nov 14 '16