Joint not able to move after adding libgazebo_ros_laser.so
I tried to make my own URDF model consist of tripod and lidar mounted 90 degrees. My intension is to move the joint between baselink and laserlink. I was able to move joint continuously, but after I adding gazebo laser plugin, the joint was not able to move anymore.
I got a warning that not appear before I adding gazebo laser plugin.
[WARN] [1586855160.781737, 29.004000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
The model is spawning and the laser scan is performed successfully. But not the joint.
Yes, I already install ros-kinetic-gazebo-ros-control
, ros-kinetic-gazebo-ros-controller
For the record, here is my urdf.xacro code:
<?xml version="1.0" ?>
<robot name="laser_urdf" xmlns:xacro="http://www.ros.org/wiki/xacro" >
<xacro:property name="M_PI" value="3.1415926535897931" />
<!-- Includes -->
<xacro:include filename="$(find laser_urdf)/urdf/link_joints.xacro" />
<m_link_box name="base_link"
origin_rpy="0 0 0" origin_xyz="0 0 0"
mass="1024"
ixx="170.667" ixy="0" ixz="0"
iyy="170.667" iyz="0"
izz="170.667"
size="0.07 0.07 0.2" />
<m_joint name="laser_joint" type="continuous"
axis_xyz="1 0 0"
origin_rpy="0 ${-M_PI/2} 0" origin_xyz="0 0 0.135"
parent="base_link"
child="laser_link" />
<m_link_mesh name="laser_link"
origin_rpy="0 0 0" origin_xyz="0 0 0"
mass="0.270"
ixx="2.632e-4" ixy="0" ixz="0"
iyy="2.632e-4" iyz="0"
izz="1.62e-4"
meshfile="package://hector_sensors_description/meshes/hokuyo_utm30lx/hokuyo_utm_30lx$(optenv TEST_SELF_FILTER).dae"
meshscale="1 1 1" />
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<alwaysOn>true</alwaysOn>
<robotNamespace>/laser_urdf</robotNamespace>
</plugin>
</gazebo>
<gazebo reference="laser_link">
<sensor type="ray" name="hokuyo_utm_30lx">
<update_rate>10</update_rate>
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<ray>
<scan>
<horizontal>
<samples>1081</samples>
<resolution>1</resolution>
<min_angle>${-135 * M_PI/180}</min_angle>
<max_angle>${135 * M_PI/180}</max_angle>
</horizontal>
<vertical>
<samples>1</samples>
<resolution>1</resolution>
<min_angle>0</min_angle>
<max_angle>0</max_angle>
</vertical>
</scan>
<range>
<min>0.2</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.004</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_hokuyo_controller" filename="libgazebo_ros_laser.so">
<robotNamespace>/laser_urdf</robotNamespace>
<topicName>/scan</topicName>
<frameName>/laser_link</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
If you wondering I make my own xacro macro:
<?xml version="1.0" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="m_joint" params="name type axis_xyz origin_rpy origin_xyz parent child">
<joint name="${name}" type="${type}">
<axis xyz="${axis_xyz}" />
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<parent link="${parent}" />
<child link="${child}" />
</joint>
<transmission name="trans_${name}">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${name}">
<!-- <hardwareInterface>EffortJointInterface</hardwareInterface> -->
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="motor_${name}">
<!-- <hardwareInterface>EffortJointInterface</hardwareInterface> -->
<!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:macro name="m_link_cylinder" params="name origin_xyz origin_rpy radius length mass ixx ixy ixz iyy iyz izz">
<link name="${name}">
<inertial>
<mass value="${mass}" />
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
</inertial>
<collision>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<cylinder radius="${radius}" length="${length}" />
</geometry>
</collision>
<visual>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<cylinder radius="${radius}" length="${length}" />
</geometry>
</visual>
</link>
</xacro:macro>
<xacro:macro name="m_link_box" params="name origin_xyz origin_rpy size mass ixx ixy ixz iyy iyz izz">
<link name="${name}">
<inertial>
<mass value="${mass}" />
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
</inertial>
<collision>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<box size="${size}" />
</geometry>
</collision>
<visual>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<box size="${size}" />
</geometry>
</visual>
</link>
</xacro:macro>
<xacro:macro name="m_link_mesh" params="name origin_xyz origin_rpy meshfile meshscale mass ixx ixy ixz iyy iyz izz">
<link name="${name}">
<inertial>
<mass value="${mass}" />
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<inertia ixx="${ixx}" ixy="${ixy}" ixz="${ixz}" iyy="${iyy}" iyz="${iyz}" izz="${izz}" />
</inertial>
<collision>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<mesh filename="${meshfile}" scale="${meshscale}"/>
</geometry>
</collision>
<visual>
<origin rpy="${origin_rpy}" xyz="${origin_xyz}" />
<geometry>
<mesh filename="${meshfile}" scale="${meshscale}"/>
</geometry>
</visual>
</link>
</xacro:macro>
</robot>
My config and my launch file are the same as in many tutorials out there. I was not able to find any solution to my problem.
Asked by Daniel Harvey on 2020-04-15 11:25:39 UTC
Comments