Exercise 3.5 Problem my_get_pose_service not published pose

Course Support ROS Navigation In 5 Days
1
  • I create a new package named my_amcl_launcher and change_map.launch
  • Create a new package named my_get_pose_service and get_pose_service.launch
    ,get_pose_service.py**
  • I try roslaunch my_amcl_launcher change_map.launch and check in rviz that poseArray/particlecloud working
  • I try rosrun my_get_pose_service get_pose_service.py. I dont see anything happen and roslaunch my_get_pose_service get_pose_service.launch not have problem with permission

This is get_pose_service.py

#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
robot_pose = Pose()

def service_callback(request):
print(“Robot Pose:”)
print(robot_pose)
return EmptyResponse() # the service Response class, in this case EmptyResponse

def sub_callback(msg):
global robot_pose
robot_pose = msg.pose.pose

rospy.init_node(‘service_server’)
my_service = rospy.Service(’/get_pose_service’, Empty , service_callback) # create the Service called get_pose_
sub_pose = rospy.Subscriber(’/amcl_pose’, PoseWithCovarianceStamped, sub_callback)
rospy.spin() # mantain the service open

3

I think the problem is in the sub_callback() function.

Your PoseWithCovariance message has the following parameteres:

Header header
PoseWithCovariance pose

you shoud access to it like so: msg.pose, instead of msg.pose.pose

4

rostopic echo -n1 /amcl_pose

header:
seq: 0
stamp:
secs: 9142
nsecs: 458000000
frame_id: “map”
pose:
pose:
position:
x: 2.194810314576936
y: -0.925938792082742
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.792869203351702
w: 0.609391849614382
covariance: [0.0016554516273927788, -8.983059637479585e-06, 0.0, 0.0, 0.0, 0.0, -8.983059637035495e-06, 0.0025703421249173575, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0013261626757992866]

node service_server working but my python script not print anything

5

The problem is that you are mixing up the message types. Look at the data inside Pose(), inside PoseWithCovariance(), PoseWithCovarianceStamped()

  • So, the /aml_pose is a geometry_msgs/PoseWithCovarianceStamped

To access the Pose the robot_base should be robot_pose = PoseWithCovarianceStamped(), and then robot_pose.pose.pose contains the position and orientation

1 Like
6

I try “rosservice call /get_pose_service” then it show output now. Is it right that I need to execute “rosservice call” as client communicate with subscriber : rosrun my_get_pose_service get_pose_service.py

Robot Pose:
position:
x: 2.1597749007281943
y: 0.6977778402093509
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.9736607289116406
w: 0.22800172143045946
Robot Pose:
position:
x: 2.1597749007281943
y: 0.6977778402093509
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.9736607289116406
w: 0.22800172143045946

7

The best way to attack the exercise is with a class:

with the __iinit__:
            1. publisher to /cmd_vel
            2. subscriber to /amcl_pose with PoseWithCovarianceStamped
            3. service proxy for /global_localization
            4. And several others: a Twist command, EmptyRequest and so  on..

          followed by methods for each:

           stop(self):

           foward(self, linear_speed, angular_speed):

           turn(self,  same as above):

           make_square(self,. ...)

           subscriber callback:

if __name__ == '__main__':

node
class object
while loop will help
call your methods

break it down into the smallest elements then put them together don’t try and do everything in one function or method it’s not possible. Also you don’t use Pose it won’t help. You do need PoseWithCovarianceStamped. If you can get something started I will work with you to help you solve the exercise and learn more at the same time.

1 Like