joint_state_publisher: UnicodeEncodeError: 'ascii' codec can't encode character u'\u0130' in position 28396: ordinal not in range(128)
Robot : Eva Robot OS: ubuntu 16.04 /kinetic
I start working on a real-state robot. i was able to link the robot on my workstation and i can see all the topics on my work station. however, for visualizing the robot on rviz i can see all the joints links but not the wheels .wheels will be separated (i attached my TF tree). for launching my real-state robot I use launch file with the urdf and two nodes (robotstatepublisher , joint_state publisher ). joint state publisher alwayes give me the below error :
Traceback (most recent call last):
File "/opt/ros/kinetic/lib/joint_state_publisher/joint_state_publisher", line 44, in <module>
jsp = joint_state_publisher.JointStatePublisher()
File "/opt/ros/kinetic/lib/python2.7/dist-packages/joint_state_publisher/__init__.py", line 157, in __init__
robot = xml.dom.minidom.parseString(description)
File "/usr/lib/python2.7/xml/dom/minidom.py", line 1928, in parseString
return expatbuilder.parseString(string)
File "/usr/lib/python2.7/xml/dom/expatbuilder.py", line 940, in parseString
return builder.parseString(string)
File "/usr/lib/python2.7/xml/dom/expatbuilder.py", line 223, in parseString
parser.Parse(string, True)
UnicodeEncodeError: 'ascii' codec can't encode character u'\u0130' in position 28396: ordinal not in range(128)
[joint_state_publisher-3] process has died [pid 7032, exit code 1, cmd /opt/ros/kinetic/lib/joint_state_publisher/joint_state_publisher __name:=joint_state_publisher
__log:=/home/moh/.ros/log/638a2d20-50f6-11ea-8fb1-983b8fb9a8e3/joint_state_publisher-3.log].
log file: /home/moh/.ros/log/638a2d20-50f6-11ea-8fb1-983b8fb9a8e3/joint_state_publisher-3*.log
My real state launch file:-
<?xml version="1.0"?>
<launch>
<arg name="robot_ns" default="/"/>
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher"/>
<param name="robot_description" textfile="$(find evarobotmodel_description)/urdf/evarobotmodel.urdf"/>
<param name="publish_frequency" type="double" value="10.0"/>
<param name="tf_prefix" type="string" value="$(arg robot_ns)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" type="bool" value="false"/>
</node>
</launch>
My TF tree:-
note: the previous Urdf works fine with gazebo . so I'm not sure if its URDf problem or joint _state _publisher issue.
kindly advise how to correctly see my robot in RVIZ and have the correct TF tree?
urdf file :
<?xml version="1.0" encoding="UTF-8"?>
<robot name="evarobotmodel">
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<gazebo reference="base_footprint">
<turnGravityOff>false</turnGravityOff>
</gazebo>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_footprint"/>
<child link="base_link"/>
</joint>
<!--BASE -->
<link name="base_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.4 0.3 0.3"/>
</geometry>
</collision>
<visual>
<origin xyz="0.065 -0.11 -0.15" rpy="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/base.dae"/>
</geometry>
<material name="Gazebo/Black"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
</inertial>
</link>
<gazebo reference="base_link">
<material>Gazebo/Black</material>
</gazebo>
<link name="basecolor_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.0001 0.0001 0.0001"/>
</geometry>
</collision>
<visual>
<origin xyz="0.065 -0.11 -0.15" rpy="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/base_orange.dae"/>
</geometry>
<material name="Orange"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.0001"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="basecolor_link">
<material>Gazebo/Orange</material>
</gazebo>
<joint name="basecolor_joint" type="fixed">
<axis xyz="0 1 0"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="basecolor_link"/>
</joint>
<!-- LEFT WHEEL -->
<link name="left_wheel_link">
<collision>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.035" radius="0.085"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/rim.dae" scale="1 1 1"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<mass value="0.85"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="left_wheel_link">
<material>Gazebo/Grey</material>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="left_wheel_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="base_link"/>
<child link="left_wheel_link"/>
<origin rpy="0 0 0" xyz="0 0.16 -0.119"/>
<limit effort="6" velocity="6"/>
<dynamics damping="0" friction="0"/>
</joint>
<link name="left_wheeltire_link">
<collision>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.001" radius="0.001"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/tire.dae"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.001"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="left_wheeltire_link">
<material>Gazebo/Black</material>
</gazebo>
<joint name="left_wheeltire_joint" type="fixed">
<axis xyz="0 1 0"/>
<parent link="left_wheel_link"/>
<child link="left_wheeltire_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
</joint>
<!-- RIGHT WHEEL -->
<link name="right_wheel_link">
<collision>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.035" radius="0.085"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/rim.dae" scale="1 1 1"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<mass value="0.85"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="right_wheel_link">
<material>Gazebo/Grey</material>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="right_wheel_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="base_link"/>
<child link="right_wheel_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.16 -0.119"/>
<limit effort="6" velocity="6"/>
<dynamics damping="0" friction="0"/>
</joint>
<link name="right_wheeltire_link">
<collision>
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.001" radius="0.001"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/tire.dae" scale="1 1 1"/>
</geometry>
<material name="Black"/>
</visual>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.001"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
</inertial>
</link>
<gazebo reference="right_wheeltire_link">
<material>Gazebo/Black</material>
</gazebo>
<joint name="right_wheeltire_joint" type="fixed">
<axis xyz="0 1 0"/>
<parent link="right_wheel_link"/>
<child link="right_wheeltire_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
</joint>
<!-- FRONT CASTER -->
<link name="front_caster_base_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.06 0.06 0.0135"/>
</geometry>
</collision>
<visual>
<origin rpy="3.1415926535897931 0 0" xyz="-0.005 -0.01075 0.00675"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/caster_base.dae" scale="1 1 1"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="front_caster_base_link">
<material>Gazebo/Grey</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="front_caster_base_joint" type="fixed">
<parent link="base_link"/>
<child link="front_caster_base_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.1604 0.0 -0.15675"/>
<axis xyz="0 0 0"/>
</joint>
<link name="front_caster_rotate_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.023" radius="0.0215"/>
</geometry>
</collision>
<visual>
<origin rpy="3.1415926535897931 0 1.57079632679" xyz="0 0 0.01"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/caster_rot.dae"/>
</geometry>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="front_caster_rotate_link">
<material>Gazebo/Grey</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="front_caster_base_rotate_joint" type="continuous">
<parent link="front_caster_base_link"/>
<child link="front_caster_rotate_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0185"/>
<axis xyz="0 0 1"/>
</joint>
<link name="front_caster_wheel_link">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0175"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0175"/>
</geometry>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="front_caster_wheel_link">
<material>Gazebo/Yellow</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="front_caster_rotate_wheel_joint" type="continuous">
<parent link="front_caster_rotate_link"/>
<child link="front_caster_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.014632 0.0 -0.0115"/>
<axis xyz="0.577350269 0.577350269 0.577350269"/>
</joint>
<!--REAR CASTER -->
<link name="rear_caster_base_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.06 0.06 0.0135"/>
</geometry>
</collision>
<visual>
<origin rpy="3.1415926535897931 0 0" xyz="-0.005 -0.01075 0.00675"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/caster_base.dae" scale="1 1 1"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="rear_caster_base_link">
<material>Gazebo/Grey</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="rear_caster_base_joint" type="fixed">
<parent link="base_link"/>
<child link="rear_caster_base_link"/>
<origin rpy="0.0 0.0 0.0" xyz="-0.1604 0.0 -0.15675"/>
<axis xyz="0 0 0"/>
</joint>
<link name="rear_caster_rotate_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.023" radius="0.0215"/>
</geometry>
</collision>
<visual>
<origin rpy="3.1415926535897931 0 1.57079632679" xyz="0 0 0.01"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/caster_rot.dae" scale="1 1 1"/>
</geometry>
<material name="Grey"/>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="rear_caster_rotate_link">
<material>Gazebo/Grey</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="rear_caster_base_rotate_joint" type="continuous">
<parent link="rear_caster_base_link"/>
<child link="rear_caster_rotate_link"/>
<origin rpy="0 0 0" xyz="0.0 0.0 -0.0185"/>
<axis xyz="0 0 1"/>
</joint>
<link name="rear_caster_wheel_link">
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0175"/>
</geometry>
</collision>
<visual>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.0175"/>
</geometry>
<material name="Yellow"/>
</visual>
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="1e-6" ixy="0.0" ixz="0.0" iyy="1e-6" iyz="0.0" izz="1e-6"/>
</inertial>
</link>
<gazebo reference="rear_caster_wheel_link">
<material>Gazebo/Yellow</material>
<mu1>0.0</mu1>
<mu2>0.0</mu2>
<kp>1000000.0</kp>
<kd>100.0</kd>
<minDepth>0.001</minDepth>
<maxVel>1.0</maxVel>
</gazebo>
<joint name="rear_caster_rotate_wheel_joint" type="continuous">
<parent link="rear_caster_rotate_link"/>
<child link="rear_caster_wheel_link"/>
<origin rpy="0 0 0" xyz="-0.014632 0.0 -0.0115"/>
<axis xyz="0.577350269 0.577350269 0.577350269"/>
</joint>
<!--Zed -->
<link name="stereoCamera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.033 0.175 0.03"/>
</geometry>
</collision>
<visual>
<origin xyz="-0.03 0 0" rpy="1.57 0 1.57"/>
<geometry>
<mesh filename="package://evarobotmodel_description/meshes/ZED-camera.stl" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<inertial>
<mass value="0.159"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="10" ixy="0" ixz="0" iyy="10" iyz="0" izz="10"/>
</inertial>
</link>
<joint name="stick2stereoCamera" type="fixed">
<origin xyz="0.17 0.0 0.1720" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="stereoCamera_link"/>
</joint>
<gazebo reference="stereoCamera_link">
<material>Gazebo/DarkGrey</material>
<sensor type="multicamera" name="stereo_camera">
<update_rate>30.0</update_rate>
<camera name="left">
<pose>0.015 0.15 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<camera name="right">
<pose>0.015 -0.15 0 0 0 0</pose>
<horizontal_fov>1.3962634</horizontal_fov>
<image>
<width>1280</width>
<height>720</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.02</near>
<far>300</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>camera</cameraName>
<imageTopicName>rgb/image_raw</imageTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<frameName>left_camera_optical_frame</frameName>
<rightFrameName>right_camera_optical_frame</rightFrameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<link name="left_camera_optical_frame"/>
<joint name="left_camera_optical_joint" type="fixed">
<origin xyz="0 0.15 0" rpy="0 0 0"/>
<parent link="stereoCamera_link"/>
<child link="left_camera_optical_frame"/>
</joint>
<link name="right_camera_optical_frame"/>
<joint name="right_camera_optical_joint" type="fixed">
<origin xyz="0 -0.15 0" rpy="0 0 0"/>
<parent link="stereoCamera_link"/>
<child link="right_camera_optical_frame"/>
</joint>
<!--Lidar -->
<!-- SONAR0 -->
<link name="sonar0_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar0_joint" type="fixed">
<origin xyz="0.130 -0.147 0.08" rpy="0 0 -1.0471975512"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar0_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar0_link">
<sensor type="ray" name="sonar0">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar0_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar0</topicName>
<frameName>sonar0_link</frameName>
<frameId>sonar0_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR1 -->
<link name="sonar1_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar1_joint" type="fixed">
<origin xyz="0.213 0.0 0.08" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar1_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar1_link">
<sensor type="ray" name="sonar1">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar1_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar1</topicName>
<frameName>sonar1_link</frameName>
<frameId>sonar1_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR2 -->
<link name="sonar2_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar2_joint" type="fixed">
<origin xyz="0.130 0.147 0.08" rpy="0 0 1.0471975512"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar2_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar2_link">
<sensor type="ray" name="sonar2">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar2_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar2</topicName>
<frameName>sonar2_link</frameName>
<frameId>sonar2_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR3 -->
<link name="sonar3_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar3_joint" type="fixed">
<origin xyz="-0.045 0.168 0.08" rpy="0 0 1.57079632679"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar3_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar3_link">
<sensor type="ray" name="sonar3">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar3_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar3</topicName>
<frameName>sonar3_link</frameName>
<frameId>sonar3_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR4 -->
<link name="sonar4_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar4_joint" type="fixed">
<origin xyz="-0.191 0.084 0.08" rpy="0 0 2.61799387799"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar4_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar4_link">
<sensor type="ray" name="sonar4">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar4_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar4</topicName>
<frameName>sonar4_link</frameName>
<frameId>sonar4_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR5 -->
<link name="sonar5_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar5_joint" type="fixed">
<origin xyz="-0.191 -0.084 0.08" rpy="0 0 -2.61799387799"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar5_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar5_link">
<sensor type="ray" name="sonar5">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar5_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar5</topicName>
<frameName>sonar5_link</frameName>
<frameId>sonar5_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- SONAR6 -->
<link name="sonar6_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="sonar6_joint" type="fixed">
<origin xyz="-0.045 -0.168 0.08" rpy="0 0 -1.57079632679"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="sonar6_link"/>
</joint>
<!-- Sonar Plugin -->
<gazebo reference="sonar6_link">
<sensor type="ray" name="sonar6">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.479965544</min_angle>
<max_angle>0.479965544</max_angle>
</horizontal>
</scan>
<range>
<min>0.02</min>
<max>5.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="sonar6_plugin" filename="libgazebo_ros_range.so">
<topicName>sonar6</topicName>
<frameName>sonar6_link</frameName>
<frameId>sonar6_link</frameId>
<radiation>ultrasound</radiation>
<fov>0.7</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- IR0 -->
<link name="ir0_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="ir0_joint" type="fixed">
<origin xyz="0.191 -0.084 -0.072" rpy="0 0 -0.52359877559"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="ir0_link"/>
</joint>
<gazebo reference="ir0_link">
<sensor type="ray" name="ir0">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.001</min_angle>
<max_angle>0.001</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>0.8</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="ir0_plugin" filename="libgazebo_ros_range.so">
<topicName>ir0</topicName>
<frameName>ir0_link</frameName>
<frameId>ir0_link</frameId>
<radiation>infrared</radiation>
<fov>0.01</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- İR1 -->
<link name="ir1_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="ir1_joint" type="fixed">
<origin xyz="0.191 0.084 -0.072" rpy="0 0 0.52359877559"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="ir1_link"/>
</joint>
<gazebo reference="ir1_link">
<sensor type="ray" name="ir1">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.001</min_angle>
<max_angle>0.001</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>0.8</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="ir1_plugin" filename="libgazebo_ros_range.so">
<topicName>ir1</topicName>
<frameName>ir1_link</frameName>
<frameId>ir1_link</frameId>
<radiation>infrared</radiation>
<fov>0.01</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- IR2 -->
<link name="ir2_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="ir2_joint" type="fixed">
<origin xyz="-0.13 0.147 -0.072" rpy="0 0 2.09439510239"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="ir2_link"/>
</joint>
<gazebo reference="ir2_link">
<sensor type="ray" name="ir2">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.001</min_angle>
<max_angle>0.001</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>0.8</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="ir2_plugin" filename="libgazebo_ros_range.so">
<topicName>ir2</topicName>
<frameName>ir2_link</frameName>
<frameId>ir2_link</frameId>
<radiation>infrared</radiation>
<fov>0.01</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- IR3 -->
<link name="ir3_link">
<inertial>
<mass value="0.001"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
</collision>
</link>
<joint name="ir3_joint" type="fixed">
<origin xyz="-0.13 -0.147 -0.072" rpy="0 0 -2.09439510239"/>
<axis xyz="1 0 0"/>
<parent link="base_link"/>
<child link="ir3_link"/>
</joint>
<gazebo reference="ir3_link">
<sensor type="ray" name="ir3">
<pose>0 0 0 0 0 0</pose>
<ray>
<scan>
<horizontal>
<samples>320</samples>
<resolution>1</resolution>
<min_angle>-0.001</min_angle>
<max_angle>0.001</max_angle>
</horizontal>
</scan>
<range>
<min>0.1</min>
<max>0.8</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name="ir3_plugin" filename="libgazebo_ros_range.so">
<topicName>ir3</topicName>
<frameName>ir3_link</frameName>
<frameId>ir3_link</frameId>
<radiation>infrared</radiation>
<fov>0.01</fov>
<gaussianNoise>0</gaussianNoise>
<updateRate>4</updateRate>
</plugin>
<always_on>1</always_on>
<visualize>false</visualize>
</sensor>
</gazebo>
<!-- BUMPER0 -->
<link name="bumper0_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.015 0.02"/>
</geometry>
</collision>
<inertial>
<mass value="1e-5"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!-- Bumper joint -->
<joint name="bumper0_joint" type="fixed">
<origin xyz="0.191 -0.084 -0.072" rpy="0 0 -0.52359877559"/>
<axis xyz="0 1 0"/>
<parent link="base_link"/>
<child link="bumper0_link"/>
</joint>
<!-- Bumper plugin -->
<gazebo reference="bumper0_link">
<sensor name="bumper0" type="contact">
<always_on>true</always_on>
<contact>
<collision>bumper0_link_collision</collision>
</contact>
<plugin name="bumper0_plugin" filename="libgazebo_ros_bumper.so">
<bumperTopicName>bumper0</bumperTopicName>
<updateRate>10</updateRate>
<frameName>bumper0_link</frameName>
</plugin>
</sensor>
</gazebo>
<!-- BUMPER1 -->
<link name="bumper1_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.015 0.02"/>
</geometry>
</collision>
<inertial>
<mass value="1e-5"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!-- Bumper joint -->
<joint name="bumper1_joint" type="fixed">
<origin xyz="0.213 0 -0.072" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<parent link="base_link"/>
<child link="bumper1_link"/>
</joint>
<!-- Bumper plugin -->
<gazebo reference="bumper1_link">
<sensor name="bumper1" type="contact">
<always_on>true</always_on>
<update_rate>10</update_rate>
<contact>
<collision>bumper1_link_collision</collision>
</contact>
<plugin name="bumper1_plugin" filename="libgazebo_ros_bumper.so">
<bumperTopicName>bumper1</bumperTopicName>
<frameName>bumper1_link</frameName>
</plugin>
</sensor>
</gazebo>
<!-- BUMPER2 -->
<link name="bumper2_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.015 0.015 0.02"/>
</geometry>
</collision>
<inertial>
<mass value="1e-5"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
</inertial>
</link>
<!-- Bumper joint -->
<joint name="bumper2_joint" type="fixed">
<origin xyz="0.191 0.084 -0.072" rpy="0 0 0.52359877559"/>
<axis xyz="0 1 0"/>
<parent link="base_link"/>
<child link="bumper2_link"/>
</joint>
<!-- Bumper plugin -->
<gazebo reference="bumper2_link">
<sensor name="bumper2" type="contact">
<always_on>true</always_on>
<update_rate>10</update_rate>
<contact>
<collision>bumper2_link_collision</collision>
</contact>
<plugin name="bumper2_plugin" filename="libgazebo_ros_bumper.so">
<bumperTopicName>bumper2</bumperTopicName>
<frameName>bumper2_link</frameName>
</plugin>
</sensor>
</gazebo>
<!-- AHRS -->
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link>
<joint name="imu_joint" type="fixed">
<axis xyz="1 0 0"/>
<origin xyz="0 0 0.19" rpy="3.1415926535897931 0 0"/>
<parent link="base_link"/>
<child link="imu_link"/>
</joint>
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<frameName>imu_link</frameName>
<bodyName>imu_link</bodyName>
<topicName>imu</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>10</updateRate>
</plugin>
</gazebo>
<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>10.0</updateRate>
<robotBaseFrame>base_footprint</robotBaseFrame>
<publishWheelTF>true</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
<wheelAcceleration>1</wheelAcceleration>
<leftJoint>right_wheel_joint</leftJoint>
<rightJoint>left_wheel_joint</rightJoint>
<wheelSeparation>0.33</wheelSeparation>
<wheelDiameter>0.17</wheelDiameter>
<wheelTorque>20</wheelTorque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotNamespace>/</robotNamespace>
<legacyMode>true</legacyMode>
<publishTf>true</publishTf>
<rosDebugLevel>na</rosDebugLevel>
<publishOdomTF>true</publishOdomTF>
</plugin>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<alwaysOn>true</alwaysOn>
<robotNamespace>/</robotNamespace>
<updateRate>1000.0</updateRate>
<legacyModeNS>true</legacyModeNS>
</plugin>
</gazebo>
<gazebo>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<robotNamespace>/</robotNamespace>
<jointName>left_wheel_joint,right_wheel_joint,front_caster_base_rotate_joint,rear_caster_base_rotate_joint,front_caster_rotate_wheel_joint,rear_caster_rotate_wheel_joint</jointName>
<updateRate>10.0</updateRate>
<alwaysOn>true</alwaysOn>
</plugin>
</gazebo>
</robot>
Asked by q8wwe on 2020-02-16 15:24:56 UTC
Answers
On line 975 of your URDF file, there is a unicode character in the comment that looks like this:
<!-- İR1 -->
The "I" has a funny dot over it. Replace it with a real "I" and you should be good. I found this by pasting your URDF file in Notepad++ which lets you search for unicode characters and searched for \u0130
, the unicode referenced in the error.
Asked by robustify on 2020-02-16 20:30:23 UTC
Comments
that's just a comment the compiler will ignore it .
Asked by q8wwe on 2020-02-17 09:58:42 UTC
A URDF file doesn't get compiled. At runtime, the joint state publisher runs it through the URDF parser to find all the joints for your robot. This parser can't handle unicode, and can only handle standard ASCII.
Asked by robustify on 2020-02-17 10:09:30 UTC
I tried to change the letter with dot , but that didn't solve joint_state_publisher issue
Asked by q8wwe on 2020-02-17 14:24:27 UTC
Are you getting the same error as before saying it is failing to parse \u0130?
Asked by robustify on 2020-02-17 14:38:14 UTC
I created new URDf by removing all parts ,I only kept tags related to base link and wheels . then i relaunched the previous real state launch file .
I got a better TF tree and I could visualize it with Rviz.
but now I'm facing new issue related to publish odom to base_link , i use robot_pose_ekf but it didn't work as when i run Rvis wheels will be separated and warning message will be displayed "No transform from [left_wheel_link] to [odom]".
Asked by q8wwe on 2020-02-17 16:23:02 UTC
That's a different issue entirely, so I recommend opening a new question for that. My guess is that the joint_state_publisher
isn't communicating properly with the robot_state_publisher
. Check an rqt_graph
.
Asked by robustify on 2020-02-17 18:20:00 UTC
Thank you for the clarification .
I made new question on this link and attached : rqt,urdf, Rviz ,reastate launch file and odom_to_base.cpp
https://answers.ros.org/question/344460/rviz-no-transform-from-links-to-odom/t
Asked by q8wwe on 2020-02-18 09:41:03 UTC
changing the letter removed that problem . thank you , many thanks to your precious help.
Asked by q8wwe on 2020-02-18 16:08:08 UTC
@robustify Thank you so much for this. I was facing a similar issue, busy looking for solutions about process died. I got no help, then later decided to check the unicode error and came across this answer. It helped greatly.
Asked by yabdulra on 2022-07-06 03:24:33 UTC
Comments