ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

How to write my own joint state

asked 2017-11-21 07:10:09 -0500

marawy_alsakaf gravatar image

updated 2017-11-24 05:08:57 -0500

Hello all i have a real robot. using ROS. i have a problem with (Joint State Publisher)


i understand that i should write my own (joint state publisher node). i read the related tutorials but i do not get how to convert encoders' messages to joint state message. i need steps or tutorial for doing that.

thanks in advance.


edit

i test the code in

my code becomes as follows

#!/usr/bin/env python
# Lucas Walter
# make a joint exactly what the command wants it to be- this only works
# for position control.

import rospy

from sensor_msgs.msg import JointState
from std_msgs.msg import Int64


class CommandToJointState:
    def __init__(self):
        self.joint_name = rospy.get_param("lwheel",True)
        self.joint_state = JointState()
        self.joint_state.name.append(self.joint_name)
        self.joint_state.position.append(0.0)
        self.joint_state.velocity.append(0.0)
        self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=1)
        self.command_sub = rospy.Subscriber("lwheel", Int64,
                                            self.command_callback, queue_size=1)


    def command_callback(self, msg):
        self.joint_state.position[0] = msg.data
        self.joint_state.header.stamp = rospy.Time.now()
        self.joint_pub.publish(self.joint_state)

if __name__ == '__main__':
    rospy.init_node('diffdrive_controller')
    diffdrive_controller = CommandToJointState()
rospy.spin()

but i get error as in the image attachedimage description

i am not familiar with python. all my codes are in C++

edit retag flag offensive close merge delete

Comments

In get_param the first argument is the name of the parameter, not the value of it- the second value is the value, you probably want get_param("joint_name", "lwheel")

lucasw gravatar image lucasw  ( 2017-11-24 08:16:12 -0500 )edit
1

@marawy_alsakaf: can you please post a copy and paste of the error instead of a screen shot? Text is searchable while images are not.

jayess gravatar image jayess  ( 2017-11-24 12:25:27 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2017-11-22 07:10:49 -0500

lucasw gravatar image

This is a very simple example in python ( https://github.com/lucasw/carbot/blob... ) :

import rospy  
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64


class CommandToJointState:
    def __init__(self):
        self.joint_name = rospy.get_param("~joint_name")
        self.joint_state = JointState()
        self.joint_state.name.append(self.joint_name)
        self.joint_state.position.append(0.0)
        self.joint_state.velocity.append(0.0)
        self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=1)
        self.command_sub = rospy.Subscriber("command", Float64,
                                            self.command_callback, queue_size=1)

    def command_callback(self, msg):
        self.joint_state.position[0] = msg.data
        self.joint_state.header.stamp = rospy.Time.now()
        self.joint_pub.publish(self.joint_state)

if __name__ == '__main__':
    rospy.init_node('command_to_joint_state')
    command_to_joint_state = CommandToJointState()
    rospy.spin()

If for example you were publishing your encoder as a Float64 (replace the 'command' topic with that encoder topic), you would change the following line to have a scale factor that converts the input encoder value to radians:

self.joint_state.position[0] = msg.data
edit flag offensive delete link more

Comments

Hi Mr. @lucasw I have a 2d laser scanner.I converted from 2d laserscan to 3d pointcloud using servo.But I want to this using dc motor with encoder.I write an arduino and python code. In my arduino code, joint_state is subscribing,In python code, angle is publishing.I guess I have to do the opposite

Mekateng gravatar image Mekateng  ( 2017-11-23 02:48:17 -0500 )edit

Mr. @lucasw My codes in here https://github.com/mekateng/ros-ardui... Could you look at it and fix the error I made? I'm very new in ros. I'm very happy if you help me.

Mekateng gravatar image Mekateng  ( 2017-11-23 03:09:44 -0500 )edit

Does the code not run? If there is an error message post it here, otherwise describe what the problem is.

lucasw gravatar image lucasw  ( 2017-11-23 07:40:40 -0500 )edit

The code is running but the encoder angle does not match the angle from the ROS exactly. so, for example there is an object in my lab ,but it looks like three on rviz in the attachment https://hizliresim.com/4G3VkQ There is a pairing problem. @lucasw Did you control my codes? is there any wrong?

Mekateng gravatar image Mekateng  ( 2017-11-23 07:50:37 -0500 )edit

There is a scale factor error somewhere in your conversion math between the radians angle and the encoder value. The easiest/laziest thing to do is to multiply the angle by 3 (or if not 3 exactly, experimentally adjust the fudge factor until it comes out right- maybe it is actually pi?).

lucasw gravatar image lucasw  ( 2017-11-23 08:33:11 -0500 )edit

And then later track down in your math where the error is introduced and fix it properly there.

lucasw gravatar image lucasw  ( 2017-11-23 08:33:35 -0500 )edit

I am sorry but I really do not understand the line I'm going to fix:/ Can you write lines? are you talking about the code in automate.py or the arduino code? @lucasw

Mekateng gravatar image Mekateng  ( 2017-11-23 12:27:28 -0500 )edit

@lucasw , i saw the example that you post. i edited it to fit my requirements. but i get an error i will edit the post to provide the image of error.

marawy_alsakaf gravatar image marawy_alsakaf  ( 2017-11-24 05:06:07 -0500 )edit

Question Tools

4 followers

Stats

Asked: 2017-11-21 07:10:09 -0500

Seen: 7,091 times

Last updated: Nov 24 '17