0
$\begingroup$

Rosanswers logo

Hi,

I want to implement a node, that calculates random values for the joints of my robot and publishes them. Furthermore, I want to call the service /gazebo/set_model_configuration to set the joints in Gazebo to these random values. I am using Ubuntu 16.04 with ROS kinetic and Gazebo 7.14.0

Here is my code:

#!/usr/bin/env python
import rospy
import numpy as np
from std_msgs.msg import Float64
from std_msgs.msg import Bool
from std_msgs.msg import Header
from sensor_msgs.msg import JointState
from gazebo_msgs.srv import SetModelConfiguration
from gazebo_msgs.srv import SetModelConfigurationRequest

joint_names = ['ShoulderJRotate', 'ShoulderJSwing', 'ElbowJSwing', 'ElbowJRotate', \
    'rh_WRJ2', 'rh_WRJ1', \
    'rh_FFJ4', 'rh_FFJ3', 'rh_FFJ2', 'rh_FFJ1', \
    'rh_MFJ4', 'rh_MFJ3', 'rh_MFJ2', 'rh_MFJ1', \
    'rh_RFJ4', 'rh_RFJ3', 'rh_RFJ2', 'rh_RFJ1', \
    'rh_LFJ5', 'rh_LFJ4', 'rh_LFJ3', 'rh_LFJ2', 'rh_LFJ1', \
    'rh_THJ5', 'rh_THJ4', 'rh_THJ3', 'rh_THJ2', 'rh_THJ1']

random_joint_states_msg = JointState()
random_joint_states_msg.header = Header()
random_joint_states_msg.name = joint_names

reset_req = SetModelConfigurationRequest()
reset_req.model_name = "shadowrobot"
reset_req.urdf_param_name = "robot_description"
reset_req.joint_names = joint_names

pub_init_joint_positions = rospy.Publisher('/init_joint_positions', JointState, queue_size=10)
reset_joints = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)

def scene_resetter():
    rospy.init_node('scene_resetter', anonymous=True)
    random_joint_positions = [np.random.uniform(-0.79, 1.57), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.52, 2.09), np.random.uniform(-1.57, 1.57), \
        np.random.uniform(-0.52, 0.17), np.random.uniform(-0.70, 0.49), \
        np.random.uniform(-0.35, 0.35), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.0, 1.57), np.random.uniform(0.0, 1.57), \
        np.random.uniform(-0.35, 0.35), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.0, 1.57), np.random.uniform(0.0, 1.57), \
        np.random.uniform(-0.35, 0.35), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.0, 1.57), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.0, 0.79), np.random.uniform(-0.35, 0.35), \
        np.random.uniform(0.0, 1.57), np.random.uniform(0.0, 1.57), \
        np.random.uniform(0.0, 1.57), np.random.uniform(-1.05, 1.05), \
        np.random.uniform(0.0, 1.22), np.random.uniform(-0.21, 0.21), \
        np.random.uniform(-0.7, 0,7), np.random.uniform(0.0, 1.57)]

    # call gazebo service
    reset_req.joint_positions = random_joint_positions
    rospy.wait_for_service("/gazebo/set_model_configuration")
    reset_joints(reset_req)

    # publish init_joint_positions
    random_joint_states_msg.position = random_joint_positions
    random_joint_states_msg.header.stamp = rospy.Time.now() 
    pub_init_joint_positions.publish(random_joint_states_msg)   
    rospy.spin()


if __name__ == '__main__':
   scene_resetter()

However when I run the node, the following error message appears in the terminal:

 $ rosrun shadowrobot_control scene_resetter.py 
Traceback (most recent call last):
  File "/home/ubuntubasic_roskinetic/shadow_try/src/shadowrobot_control/scripts/scene_resetter.py", line 83, in <module>
    scene_resetter()
  File "/home/ubuntubasic_roskinetic/shadow_try/src/shadowrobot_control/scripts/scene_resetter.py", line 73, in scene_resetter
    reset_joints(reset_req)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 512, in call
    transport.send_message(request, self.seq)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_base.py", line 665, in send_message
    serialize_message(self.write_buff, seq, msg)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/msg.py", line 152, in serialize_message
    msg.serialize(b)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/gazebo_msgs/srv/_SetModelConfiguration.py", line 90, in serialize
    except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(locals().get('_x', self)))))
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/genpy/message.py", line 336, in _check_types
    raise SerializationError(str(exc))
genpy.message.SerializationError: <class 'struct.error'>: 'required argument is not a float' when writing 'robot_description'

What am I doing wrong? How can I resolve this?

Thank you in advance!

Best, Verena


Originally posted by verena on ROS Answers with karma: 17 on 2018-09-21

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

According to the documentation (here), the joint_positions field is a list of float64 elements.

You appear to be assigninging a list of numpy types to it.

That is ok with Python (as the interpreter itself doesn't care), but genpy can't work with those, hence the error.


Edit:

Now I converted my numpy types to Float64, but the error remains.

Float64 is a ROS msg type that wraps float64 primitive types.

And float64 is not a Python type. If you use float(..) it should work.


Originally posted by gvdhoorn with karma: 86574 on 2018-09-21

This answer was ACCEPTED on the original site

Post score: 0


Original comments

Comment by verena on 2018-09-21:
Thank you so much. Converting types did the trick! Because of the last line of the error message, I thought my problem was the type of 'robot_description'. Obviously, my guess was wrong. :)

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.