velocity got from /joint_states is error?
Also asked here : https://github.com/ros-simulation/gazebo_ros_pkgs/issues/830
With hardware_interface/EffortJointInterface, I published the effort commands as follows:
```
import rospy
from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped, PoseArray, Pose
from std_msgs.msg import Float64
class CmdPub(object):
def __init__(self):
self.joints_pub=rospy.Publisher('my_effort_controller/command', Float64, queue_size=1)
def function_pub_effort_elfin5(self, effort):
self.joints_pub.publish(effort)
if __name__=='__main__':
rospy.init_node('cmd_pub', anonymous=True)
cp = CmdPub()
effort = -6
while True:
effort = -1 * effort
cp.function_pub_effort_elfin5(effort)
rospy.sleep(5)
print "effort ", effort
rospy.spin()
```
And I subscribed /joint_states and plot position and velocity line:
```
rospy.Subscriber('/joint_states', JointState, joint_states_cb)
```
The line I got is:
figure_1
But It seems the velocity is not currect. I caculated the velocity and got this:
figure_1-2
Are there some error with these codes:
```
void GazeboRosJointStatePublisher::publishJointStates() {
ros::Time current_time = ros::Time::now();
joint_state_.header.stamp = current_time;
joint_state_.name.resize ( joints_.size() );
joint_state_.position.resize ( joints_.size() );
joint_state_.velocity.resize ( joints_.size() );
for ( int i = 0; i < joints_.size(); i++ ) {
physics::JointPtr joint = joints_[i];
double velocity = joint->GetVelocity( 0 );
#if GAZEBO_MAJOR_VERSION >= 8
double position = joint->Position ( 0 );
#else
double position = joint->GetAngle ( 0 ).Radian();
#endif
joint_state_.name[i] = joint->GetName();
joint_state_.position[i] = position;
joint_state_.velocity[i] = velocity;
}
joint_state_publisher_.publish ( joint_state_ );
}
```
↧