syntax of a srv file
Hi I am trying to write a service that uses the data that the drc robot is publishing on the imu topic. if i write rostopic type /imu | rosmsg show , I get -
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
float64 x
float64 y
float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
float64 x
float64 y
float64 z
float64[9] linear_acceleration_covariance
I tried this-
sensor_msgs/Imu imu
---
float64 ubx
float64 mby
the package compiled but the server isn't working. the files are Server
#!/usr/bin/env python
#####################################################################################
import roslib;
roslib.load_manifest('imu')
from imu.srv import *
from imu.msg import *
import rospy, math , sys
from std_msgs.msg import Float64
from geometry_msgs.msg import Vector3
from sensor_msgs.msg import *
class Namespace2: pass
ns = Namespace2()
ns.old_time_nsec=0.0
ns.old_time_sec=0.0
def imu_controller(req):
res= ImuAngResponse()
x_vel=req.imu.angular_velocity.x
y_vel=req.imu.angular_velocity.y
z_vel=req.imu.angular_velocity.z
ns.new_time_nsec=req.header.stamp.nsecs
ns.new_time_sec=req.header.stamp.secs
new_time=ns.new_time_sec+ns.new_time_nsec*10^-9
old_time=ns.old_time_sec+ns.old_time_nsec*10^-9
dt= new_time-old_time
res.ubx=x_vel*dt
res.mby=y_vel*dt
ns.old_time_nsec=ns.new_time_nsec
ns.old_time_sec=ns.new_time_sec
return res
def imu_controller_server():
rospy.init_node('imu_controller_server')
s = rospy.Service('imu_controller', ImuAng, imu_controller)
rospy.loginfo("imu_controller server ready")
rospy.spin()
if __name__ == "__main__":
imu_controller_server()
Caller
#!/usr/bin/env python
#####################################################################################
import roslib;
roslib.load_manifest('imu')
from imu.srv import *
from imu.msg import *
import rospy, math , sys
from std_msgs.msg import Float64
from geometry_msgs.msg import Vector3
from sensor_msgs.msg import *
class Nasmpace1: pass
ns = Nasmpace1()
ns.ImuAng = ImuAngResponse()
imu_controller = rospy.ServiceProxy('imu_controller', ImuAng)
def imu_control(msg):
ns.ImuAng=imu_controller(msg)
ns.back_ubx.publish(ns.ImuAng.ubx)
ns.back_mby.publish(ns.ImuAng.mby)
def imu_call():
rospy.init_node('imu_call')
rospy.wait_for_service('imu_controller')
ns.back_ubx = rospy.Publisher('/back_ubx_position_controller/command', Float64)
ns.back_mby = rospy.Publisher('/back_mby_position_controller/command', Float64)
rospy.loginfo( "imu_call node is ready" )
sub=rospy.Subscriber("imu", Imu, imu_control)
rospy.spin()
if __name__ == '__main__':
imu_call()
the srv file is ImuAng.srv
sensor_msgs/Imu imu
---
float64 ubx
float64 mby
the error I am getting is
[ERROR] [WallTime: 1357222067.922667] [4099.311000] Error processing request: 'ImuAngRequest' object has no attribute 'header'
None
Traceback (most recent call last):
File "/opt/ros/fuerte/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 615, in _handle_request
response = convert_return_to_response(self.handler(request), self.response_class)
File "/home/nachum/ros/imu/scripts/imu_fix_server.py", line 27, in imu_controller
ns.new_time_nsec=req.header.stamp.nsecs
AttributeError: 'ImuAngRequest' object has no attribute 'header'
what is the right syntax?( I only need the time and angular velocity)
Thanks
Nachum
add a comment