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