husky / husky

Common packages for the Clearpath Husky
http://wiki.ros.org/Robots/Husky
BSD 3-Clause "New" or "Revised" License
447 stars 432 forks source link

Gmapping does not work #243

Closed PhilipAmadasun closed 2 years ago

PhilipAmadasun commented 2 years ago

Please provide the following information:

To Reproduce Provide the steps to reproduce: -I simply followed the instructions in the husky ROS Wiki here

Other notes Here is my husky.urdf.xacro file:

<?xml version="1.0"?>
<!--
Software License Agreement (BSD)

\file      husky.urdf.xacro
\authors   Paul Bovbel <pbovbel@clearpathrobotics.com>, Devon Ash <dash@clearpathrobotics.com>
\copyright Copyright (c) 2015, Clearpath Robotics, Inc., All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that
the following conditions are met:
 * Redistributions of source code must retain the above copyright notice, this list of conditions and the
   following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the
   following disclaimer in the documentation and/or other materials provided with the distribution.
 * Neither the name of Clearpath Robotics nor the names of its contributors may be used to endorse or promote
   products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WAR-
RANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, IN-
DIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<robot name="husky" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED 0)" />
  <xacro:arg name="laser_xyz" default="$(optenv HUSKY_LMS1XX_XYZ 0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_rpy" default="$(optenv HUSKY_LMS1XX_RPY 0.0 0.0 0.0)" />

  <xacro:arg name="laser_secondary_enabled" default="$(optenv HUSKY_LMS1XX_SECONDARY_ENABLED 0)" />
  <xacro:arg name="laser_secondary_xyz" default="$(optenv HUSKY_LMS1XX_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_secondary_rpy" default="$(optenv HUSKY_LMS1XX_SECONDARY_RPY 0.0 0.0 3.14159)" />

  <xacro:arg name="laser_ust10_front_enabled" default="$(optenv HUSKY_UST10_ENABLED 1)" />
  <xacro:arg name="laser_ust10_front_prefix"  default="$(optenv HUSKY_UST10_PREFIX front)" />
  <xacro:arg name="laser_ust10_front_parent"  default="$(optenv HUSKY_UST10_PARENT top_plate_link)" />
  <xacro:arg name="laser_ust10_front_topic"   default="$(optenv HUSKY_UST10_TOPIC front/scan)" />
  <xacro:arg name="laser_ust10_front_xyz"     default="$(optenv HUSKY_UST10_XYZ 0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_ust10_front_rpy"     default="$(optenv HUSKY_UST10_RPY 0 0 0)" />

  <xacro:arg name="laser_ust10_rear_enabled"  default="$(optenv HUSKY_UST10_SECONDARY_ENABLED 0)" />
  <xacro:arg name="laser_ust10_rear_prefix"   default="$(optenv HUSKY_UST10_SECONDARY_PREFIX rear)" />
  <xacro:arg name="laser_ust10_rear_parent"   default="$(optenv HUSKY_UST10_SECONDARY_PARENT top_plate_link)" />
  <xacro:arg name="laser_ust10_rear_topic"    default="$(optenv HUSKY_UST10_SECONDARY_TOPIC rear/scan)" />
  <xacro:arg name="laser_ust10_rear_xyz"      default="$(optenv HUSKY_UST10_SECONDARY_XYZ -0.2206 0.0 0.00635)" />
  <xacro:arg name="laser_ust10_rear_rpy"      default="$(optenv HUSKY_UST10_SECONDARY_RPY 0 0 3.14159)" />

  <xacro:arg name="laser_3d_enabled" default="$(optenv HUSKY_LASER_3D_ENABLED 0)" />
  <xacro:arg name="laser_3d_xyz" default="$(optenv HUSKY_LASER_3D_XYZ 0 0 0)" />
  <xacro:arg name="laser_3d_rpy" default="$(optenv HUSKY_LASER_3D_RPY 0 0 0)" />

  <xacro:arg name="realsense_enabled" default="$(optenv HUSKY_REALSENSE_ENABLED 0)" />
  <xacro:arg name="realsense_xyz" default="$(optenv HUSKY_REALSENSE_XYZ 0 0 0)" />
  <xacro:arg name="realsense_rpy" default="$(optenv HUSKY_REALSENSE_RPY 0 0 0)" />
  <xacro:arg name="realsense_mount" default="$(optenv HUSKY_REALSENSE_MOUNT_FRAME sensor_arch_mount_link)" />

  <xacro:property name="husky_front_bumper_extend" value="$(optenv HUSKY_FRONT_BUMPER_EXTEND 0)" />
  <xacro:property name="husky_rear_bumper_extend" value="$(optenv HUSKY_REAR_BUMPER_EXTEND 0)" />

  <!-- Height of the sensor arch in mm.  Must be either 510 or 300 -->
  <xacro:arg name="sensor_arch_height"  default="$(optenv HUSKY_SENSOR_ARCH_HEIGHT 510)" />
  <xacro:arg name="sensor_arch"         default="$(optenv HUSKY_SENSOR_ARCH 0)" />

  <xacro:arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE /)" />
  <xacro:arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS empty.urdf)" />
  <xacro:arg name="cpr_urdf_extras" default="$(optenv CPR_URDF_EXTRAS empty.urdf)" />

  <!-- Included URDF/XACRO Files -->
  <xacro:include filename="$(find husky_description)/urdf/decorations.urdf.xacro" />
  <xacro:include filename="$(find husky_description)/urdf/wheel.urdf.xacro" />

  <xacro:include filename="$(find husky_description)/urdf/accessories/intel_realsense.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/sick_lms1xx_mount.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/sensor_arch.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/hokuyo_ust10.urdf.xacro"/>
  <xacro:include filename="$(find husky_description)/urdf/accessories/vlp16_mount.urdf.xacro"/>

  <xacro:property name="M_PI" value="3.14159"/>

  <!-- Base Size -->
  <xacro:property name="base_x_size" value="0.98740000" />
  <xacro:property name="base_y_size" value="0.57090000" />
  <xacro:property name="base_z_size" value="0.24750000" />

  <!-- Wheel Mounting Positions -->
  <xacro:property name="wheelbase" value="0.5120" />
  <xacro:property name="track" value="0.5708" />
  <xacro:property name="wheel_vertical_offset" value="0.03282" />

  <!-- Wheel Properties -->
  <xacro:property name="wheel_length" value="0.1143" />
  <xacro:property name="wheel_radius" value="0.1651" />

  <!-- Base link is the center of the robot's bottom plate -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://husky_description/meshes/base_link.dae" />
      </geometry>
    </visual>
    <collision>
      <origin xyz="${( husky_front_bumper_extend - husky_rear_bumper_extend ) / 2.0} 0 ${base_z_size/4}" rpy="0 0 0" />
      <geometry>
        <box size="${ base_x_size + husky_front_bumper_extend + husky_rear_bumper_extend } ${base_y_size} ${base_z_size/2}"/>
      </geometry>
    </collision>
    <collision>
      <origin xyz="0 0 ${base_z_size*3/4-0.01}" rpy="0 0 0" />
      <geometry>
        <box size="${base_x_size*4/5} ${base_y_size} ${base_z_size/2-0.02}"/>
      </geometry>
    </collision>
  </link>

  <!-- Base footprint is on the ground under the robot -->
  <link name="base_footprint"/>

  <joint name="base_footprint_joint" type="fixed">
    <origin xyz="0 0 ${wheel_vertical_offset - wheel_radius}" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="base_footprint" />
  </joint>

  <!-- Inertial link stores the robot's inertial information -->
  <link name="inertial_link">
    <inertial>
      <mass value="46.034" />
      <origin xyz="-0.00065 -0.085 0.062" />
      <inertia ixx="0.6022" ixy="-0.02364" ixz="-0.1197" iyy="1.7386" iyz="-0.001544" izz="2.0296" />
    </inertial>
  </link>

  <joint name="inertial_joint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link" />
    <child link="inertial_link" />
  </joint>

  <!-- IMU Link is the standard mounting position for the UM6 IMU.-->
  <!-- Can be modified with environment variables in /etc/ros/setup.bash -->
  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <origin xyz="$(optenv HUSKY_IMU_XYZ 0.19 0 0.149)" rpy="$(optenv HUSKY_IMU_RPY 0 -1.5708 3.1416)" />
    <parent link="base_link" />
    <child link="imu_link" />
  </joint>
  <gazebo reference="imu_link">
  </gazebo>

  <!-- Husky wheel macros -->
  <xacro:husky_wheel wheel_prefix="front_left">
    <origin xyz="${wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="front_right">
    <origin xyz="${wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="rear_left">
    <origin xyz="${-wheelbase/2} ${track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>
  <xacro:husky_wheel wheel_prefix="rear_right">
    <origin xyz="${-wheelbase/2} ${-track/2} ${wheel_vertical_offset}" rpy="0 0 0" />
  </xacro:husky_wheel>

  <xacro:husky_decorate />

  <!--
    Add the primary and secondary lasers
  -->
  <xacro:if value="$(arg laser_enabled)">
    <xacro:sick_lms1xx_mount prefix="base"/>

    <xacro:sick_lms1xx frame="base_laser" topic="scan" robot_namespace="$(arg robot_namespace)"/>

    <joint name="laser_mount_joint" type="fixed">
      <origin xyz="$(arg laser_xyz)" rpy="$(arg laser_rpy)" />
      <parent link="top_plate_link" />
      <child link="base_laser_mount" />
    </joint>
  </xacro:if>
  <xacro:if value="$(arg laser_secondary_enabled)">
    <xacro:sick_lms1xx_mount prefix="rear"/>

    <xacro:sick_lms1xx frame="rear_laser" topic="rear/scan" robot_namespace="$(arg robot_namespace)"/>

    <joint name="laser_secondary_mount_joint" type="fixed">
      <origin xyz="$(arg laser_secondary_xyz)" rpy="$(arg laser_secondary_rpy)" />
      <parent link="top_plate_link" />
      <child link="rear_laser_mount" />
    </joint>
  </xacro:if>

  <!-- 
    Add front and back hokuyo UST10 Lidars
  -->
  <xacro:if value="$(arg laser_ust10_front_enabled)">
    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_front_topic)" prefix="$(arg laser_ust10_front_prefix)" parent_link="$(arg laser_ust10_front_parent)">
        <origin xyz="$(arg laser_ust10_front_xyz)" rpy="$(arg laser_ust10_front_rpy)" />
      </xacro:hokuyo_ust10_mount>
  </xacro:if>
  <xacro:if value="$(arg laser_ust10_rear_enabled)">
    <xacro:hokuyo_ust10_mount topic="$(arg laser_ust10_rear_topic)" prefix="$(arg laser_ust10_rear_prefix)" parent_link="$(arg laser_ust10_rear_parent)">
        <origin xyz="$(arg laser_ust10_rear_xyz)" rpy="$(arg laser_ust10_rear_rpy)" />
      </xacro:hokuyo_ust10_mount>
  </xacro:if>

  <!--
    Add the main sensor arch if the user has specifically enabled it, or if a sensor
    requires it for mounting
  -->
  <xacro:property name="sensorbar_user_enabled"     value="$(arg sensor_arch)" />
  <xacro:property name="sensorbar_needed_realsense" value="$(arg realsense_enabled)" />
  <xacro:property name="sensorbar_needed_lidar"     value="$(arg laser_3d_enabled)" />
  <xacro:if value="${sensorbar_needed_realsense or sensorbar_user_enabled or sensorbar_needed_lidar}">
    <xacro:sensor_arch prefix="" parent="top_plate_link" size="$(arg sensor_arch_height)">
        <origin xyz="$(optenv HUSKY_SENSOR_ARCH_OFFSET 0 0 0)" rpy="$(optenv HUSKY_SENSOR_ARCH_RPY 0 0 0)"/>
      </xacro:sensor_arch>
  </xacro:if>

  <!-- add the intel realsense to the sensor arch if needed -->
  <xacro:if value="$(arg realsense_enabled)">
    <link name="realsense_mountpoint"/>
    <joint name="realsense_mountpoint_joint" type="fixed">
      <origin xyz="$(arg realsense_xyz)" rpy="$(arg realsense_rpy)" />
      <parent link="$(arg realsense_mount)"/>
      <child link="realsense_mountpoint" />
    </joint>
    <xacro:intel_realsense_mount prefix="camera" topic="realsense" parent_link="realsense_mountpoint"/>
  </xacro:if>

  <!--
    Add the 3d laser to the sensor arch if needed
  -->
  <xacro:if value="$(arg laser_3d_enabled)">
    <xacro:vlp16_mount prefix="" parent_link="sensor_arch_mount_link" topic="$(optenv HUSKY_LASER_3D_TOPIC points)">
      <origin xyz="$(arg laser_3d_xyz)" rpy="$(arg laser_3d_rpy)" />
    </xacro:vlp16_mount>
  </xacro:if>

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="imu_controller" filename="libhector_gazebo_ros_imu.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <updateRate>50.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>imu/data</topicName>
      <accelDrift>0.005 0.005 0.005</accelDrift>
      <accelGaussianNoise>0.005 0.005 0.005</accelGaussianNoise>
      <rateDrift>0.005 0.005 0.005 </rateDrift>
      <rateGaussianNoise>0.005 0.005 0.005 </rateGaussianNoise>
      <headingDrift>0.005</headingDrift>
      <headingGaussianNoise>0.005</headingGaussianNoise>
    </plugin>
  </gazebo>

  <gazebo>
    <plugin name="gps_controller" filename="libhector_gazebo_ros_gps.so">
      <robotNamespace>$(arg robot_namespace)</robotNamespace>
      <updateRate>40</updateRate>
      <bodyName>base_link</bodyName>
      <frameId>base_link</frameId>
      <topicName>navsat/fix</topicName>
      <velocityTopicName>navsat/vel</velocityTopicName>
      <referenceLatitude>49.9</referenceLatitude>
      <referenceLongitude>8.9</referenceLongitude>
      <referenceHeading>0</referenceHeading>
      <referenceAltitude>0</referenceAltitude>
      <drift>0.0001 0.0001 0.0001</drift>
    </plugin>
  </gazebo>

  <!-- Optional custom includes. -->
  <xacro:include filename="$(arg urdf_extras)" />

  <!-- Optional for Clearpath internal softwares -->
  <xacro:include filename="$(arg cpr_urdf_extras)" />

</robot>

Here is my gmapping_demo.launch file:

<?xml version="1.0"?>

<launch>
  <arg name="scan_topic" default="$(eval optenv('HUSKY_LASER_TOPIC', 'scan'))" />

  <!--- Run gmapping -->
  <include file="$(find husky_navigation)/launch/gmapping.launch">
    <arg name="scan_topic" value="$(arg scan_topic)" />
  </include>

  <!--- Run Move Base -->
  <include file="$(find husky_navigation)/launch/move_base.launch" />

</launch>
gxfgit commented 2 years ago

您好,邮件已经收到,我会尽快查看并回复。谢谢! 

civerachb-cpr commented 2 years ago

What are the error messages you're seeing in Rviz? A screenshot of Rviz with the errors expanded would be helpful. Can you run

roslaunch husky_viz view_model.launch

and see the Husky model rendering correctly?

When you start Gazebo are you seeing any error messages in the terminal?

Which branch did you check out when you built from source and did you install the additional dependencies by running

rosdep install --from-paths src --ignore-src -r -y

inside your catkin workspace folder before running catkin_make?

PhilipAmadasun commented 2 years ago

@civerachb-cpr @tonybaltovski Yes I see the husky model rendering correclty. This is what my terminal looks like when I run gazebo( roslaunch husky_gazebo husky_playpen.launch):

SUMMARY
========

PARAMETERS
 * /ekf_localization/base_link_frame: base_link
 * /ekf_localization/frequency: 50
 * /ekf_localization/imu0: imu/data
 * /ekf_localization/imu0_config: [False, False, Fa...
 * /ekf_localization/imu0_differential: True
 * /ekf_localization/imu0_queue_size: 10
 * /ekf_localization/imu0_remove_gravitational_acceleration: True
 * /ekf_localization/odom0: husky_velocity_co...
 * /ekf_localization/odom0_config: [False, False, Fa...
 * /ekf_localization/odom0_differential: False
 * /ekf_localization/odom0_queue_size: 10
 * /ekf_localization/odom_frame: odom
 * /ekf_localization/predict_to_current_time: True
 * /ekf_localization/two_d_mode: True
 * /ekf_localization/world_frame: odom
 * /gazebo/enable_ros_network: True
 * /husky_joint_publisher/publish_rate: 50
 * /husky_joint_publisher/type: joint_state_contr...
 * /husky_velocity_controller/angular/z/has_acceleration_limits: True
 * /husky_velocity_controller/angular/z/has_velocity_limits: True
 * /husky_velocity_controller/angular/z/max_acceleration: 6.0
 * /husky_velocity_controller/angular/z/max_velocity: 2.0
 * /husky_velocity_controller/base_frame_id: base_link
 * /husky_velocity_controller/cmd_vel_timeout: 0.25
 * /husky_velocity_controller/enable_odom_tf: False
 * /husky_velocity_controller/estimate_velocity_from_position: False
 * /husky_velocity_controller/left_wheel: ['front_left_whee...
 * /husky_velocity_controller/linear/x/has_acceleration_limits: True
 * /husky_velocity_controller/linear/x/has_velocity_limits: True
 * /husky_velocity_controller/linear/x/max_acceleration: 3.0
 * /husky_velocity_controller/linear/x/max_velocity: 1.0
 * /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/publish_rate: 50
 * /husky_velocity_controller/right_wheel: ['front_right_whe...
 * /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/type: diff_drive_contro...
 * /husky_velocity_controller/velocity_rolling_window_size: 2
 * /husky_velocity_controller/wheel_radius_multiplier: 1.0
 * /husky_velocity_controller/wheel_separation_multiplier: 1.875
 * /joy_teleop/joy_node/autorepeat_rate: 20
 * /joy_teleop/joy_node/deadzone: 0.1
 * /joy_teleop/joy_node/dev: /dev/input/ps4
 * /joy_teleop/teleop_twist_joy/axis_angular: 0
 * /joy_teleop/teleop_twist_joy/axis_linear: 1
 * /joy_teleop/teleop_twist_joy/enable_button: 4
 * /joy_teleop/teleop_twist_joy/enable_turbo_button: 5
 * /joy_teleop/teleop_twist_joy/scale_angular: 1.4
 * /joy_teleop/teleop_twist_joy/scale_linear: 0.4
 * /joy_teleop/teleop_twist_joy/scale_linear_turbo: 2.0
 * /robot_description: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /twist_mux/locks: [{'topic': 'e_sto...
 * /twist_mux/topics: [{'topic': 'joy_t...
 * /use_sim_time: True

NODES
  /
    base_controller_spawner (controller_manager/spawner)
    ekf_localization (robot_localization/ekf_localization_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_husky_model (gazebo_ros/spawn_model)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)
  /joy_teleop/
    joy_node (joy/joy_node)
    teleop_twist_joy (teleop_twist_joy/teleop_node)

auto-starting new master
process[master]: started with pid [3547]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 9e17cb0a-1f4d-11ed-a3ef-d43b04207196
process[rosout-1]: started with pid [3558]
started core service [/rosout]
process[gazebo-2]: started with pid [3561]
process[gazebo_gui-3]: started with pid [3566]
process[base_controller_spawner-4]: started with pid [3571]
process[ekf_localization-5]: started with pid [3578]
process[twist_marker_server-6]: started with pid [3583]
process[robot_state_publisher-7]: started with pid [3587]
process[twist_mux-8]: started with pid [3588]
process[joy_teleop/joy_node-9]: started with pid [3589]
process[joy_teleop/teleop_twist_joy-10]: started with pid [3606]
process[spawn_husky_model-11]: started with pid [3611]
[ERROR] [1660865378.071367749]: Couldn't open joystick /dev/input/ps4. Will retry every second.
[ INFO] [1660865378.077141914]: [twist_marker_server] Initialized.
[ INFO] [1660865391.345971919]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1660865391.348159026]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1660865391.353317484]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1660865391.353910831]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1660865393.497576157]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1660865393.526689468]: Physics dynamic reconfigure ready.
[ WARN] [1660865393.935374807, 1298.480000000]: Failed to meet update rate! Took 1298.4800000000000182
[ WARN] [1660865393.936253744, 1298.480000000]: Failed to meet update rate! Took 1298.4600000000000364
[ERROR] [1660865393.938071, 1298.480000]: Spawn service failed. Exiting.
Warning [parser_urdf.cc:1115] multiple inconsistent <gravity> exists due to fixed joint reduction overwriting previous value [true] with [false].
[spawn_husky_model-11] process has died [pid 3611, exit code 1, cmd /opt/ros/melodic/lib/gazebo_ros/spawn_model -x 0.0 -y 0.0 -z 0.0 -Y 0.0 -unpause -urdf -param robot_description -model husky -robot_namespace / __name:=spawn_husky_model __log:=/home/philip/.ros/log/9e17cb0a-1f4d-11ed-a3ef-d43b04207196/spawn_husky_model-11.log].
log file: /home/philip/.ros/log/9e17cb0a-1f4d-11ed-a3ef-d43b04207196/spawn_husky_model-11*.log
[ INFO] [1660865394.334984521, 1298.480000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1660865394.335077386, 1298.480000000]: Starting Laser Plugin (ns = /)
[ INFO] [1660865394.492097856, 1298.480000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1660865394.516427930, 1298.480000000]: Loading gazebo_ros_control plugin
[ INFO] [1660865394.516529508, 1298.480000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1660865394.517201721, 1298.480000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1660865394.821488355, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1660865394.822843685, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1660865394.824145949, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1660865394.825384833, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
[ INFO] [1660865395.003070484, 1298.480000000]: Loaded gazebo_ros_control.
[ INFO] [1660865395.277708211, 1298.480000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1660865395.277801749, 1298.480000000]: imu plugin missing <rpyOffset>, defaults to 0s
[ INFO] [1660865395.465279705, 1298.480000000]: Controller state will be published at 50Hz.
[ INFO] [1660865395.468016649, 1298.480000000]: Wheel separation will be multiplied by 1.875.
[ INFO] [1660865395.469612307, 1298.480000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1660865395.469687033, 1298.480000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1660865395.471477111, 1298.480000000]: Velocity rolling window size of 2.
[ INFO] [1660865395.473070743, 1298.480000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1660865395.474013814, 1298.480000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1660865395.475600114, 1298.480000000]: Base frame_id set to base_link
[ INFO] [1660865395.476336266, 1298.480000000]: Odometry frame_id set to odom
[ INFO] [1660865395.478097754, 1298.480000000]: Publishing to tf is disabled
[ INFO] [1660865395.506045286, 1298.480000000]: left wheel to origin: 0.256,0.2854, 0.03282
[ INFO] [1660865395.506132354, 1298.480000000]: right wheel to origin: 0.256,-0.2854, 0.03282
[ INFO] [1660865395.506288286, 1298.480000000]: Odometry params : wheel separation 1.07025, left wheel radius 0.1651, right wheel radius 0.1651
[ INFO] [1660865395.510639017, 1298.480000000]: Adding left wheel with joint name: front_left_wheel and right wheel with joint name: front_right_wheel
[ INFO] [1660865395.510798719, 1298.480000000]: Adding left wheel with joint name: rear_left_wheel and right wheel with joint name: rear_right_wheel
[ INFO] [1660865395.536136459, 1298.480000000]: Dynamic Reconfigure:
DynamicParams:
    Odometry parameters:
        left wheel radius multiplier: 1
        right wheel radius multiplier: 1
        wheel separation multiplier: 1.875
    Publication parameters:
        Publish executed velocity command: disabled
        Publication rate: 50
        Publish frame odom on tf: disabled
[Err] [REST.cc:205] Error in REST request

libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'
[ WARN] [1660865805.610883380, 1708.100000000]: Shutdown request received.
[ WARN] [1660865805.613278745, 1708.110000000]: Reason given for shutdown: [[/robot_state_publisher] Reason: new node registered with same name]
[robot_state_publisher-7] process has finished cleanly
log file: /home/philip/.ros/log/9e17cb0a-1f4d-11ed-a3ef-d43b04207196/robot_state_publisher-7*.log

When I roslaunch husky_viz view_robot.launch. The terminal looks like this:

started roslaunch server http://localhost:42799/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    rviz (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[rviz-1]: started with pid [5670]
[ INFO] [1660866708.155783504]: rviz version 1.13.24
[ INFO] [1660866708.155825845]: compiled against Qt version 5.9.5
[ INFO] [1660866708.155837846]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1660866708.161636304]: Forcing OpenGl version 0.
[ INFO] [1660866708.274398128]: Stereo is NOT SUPPORTED
[ INFO] [1660866708.274540067]: OpenGL device: Mesa DRI Intel(R) UHD Graphics 630 (Coffeelake 3x8 GT2) 
[ INFO] [1660866708.274614987]: OpenGl version: 3.0 (GLSL 1.3).
[ WARN] [1660866708.950775989, 1309.200000000]: Interactive marker 'robot_twist_marker' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.

This is what my rviz looks like: husky )

PhilipAmadasun commented 2 years ago

@civerachb-cpr @tonybaltovski I did not build any of the husky packages in a workspace. I used sudo apt-get install ros-melodic-husky*

civerachb-cpr commented 2 years ago

This part of your log looks suspicious:

[ WARN] [1660865805.610883380, 1708.100000000]: Shutdown request received.
[ WARN] [1660865805.613278745, 1708.110000000]: Reason given for shutdown: [[/robot_state_publisher] Reason: new node registered with same name]
[robot_state_publisher-7] process has finished cleanly

It looks like other ROS nodes may already be running on your system; either another simulation or the nodes needed for a physical robot. Do you have any other ROS nodes running on the computer? Or have you modified any launch files to start an additional robot_state_publisher node?

I just installed Ubuntu 18.04 in a VM, added the ROS and Clearpath apt + rosdep sources, and ran

sudo apt-get install ros-melodic-husky*

followed by

roslaunch husky_gazebo husky_playpen.launch

I'm seeing this output, much of which is the same as yours, but crucially without the warning about duplicate robot_state_publisher nodes:

$ roslaunch husky_gazebo husky_playpen.launch 
... logging to /home/administrator/.ros/log/450eef54-1ffa-11ed-aec0-080027d6af03/roslaunch-cpr-cib-vm-melodic-8677.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://cpr-cib-vm-melodic:36449/

SUMMARY
========

PARAMETERS
 * /ekf_localization/base_link_frame: base_link
 * /ekf_localization/frequency: 50
 * /ekf_localization/imu0: imu/data
 * /ekf_localization/imu0_config: [False, False, Fa...
 * /ekf_localization/imu0_differential: True
 * /ekf_localization/imu0_queue_size: 10
 * /ekf_localization/imu0_remove_gravitational_acceleration: True
 * /ekf_localization/odom0: husky_velocity_co...
 * /ekf_localization/odom0_config: [False, False, Fa...
 * /ekf_localization/odom0_differential: False
 * /ekf_localization/odom0_queue_size: 10
 * /ekf_localization/odom_frame: odom
 * /ekf_localization/predict_to_current_time: True
 * /ekf_localization/two_d_mode: True
 * /ekf_localization/world_frame: odom
 * /gazebo/enable_ros_network: True
 * /husky_joint_publisher/publish_rate: 50
 * /husky_joint_publisher/type: joint_state_contr...
 * /husky_velocity_controller/angular/z/has_acceleration_limits: True
 * /husky_velocity_controller/angular/z/has_velocity_limits: True
 * /husky_velocity_controller/angular/z/max_acceleration: 6.0
 * /husky_velocity_controller/angular/z/max_velocity: 2.0
 * /husky_velocity_controller/base_frame_id: base_link
 * /husky_velocity_controller/cmd_vel_timeout: 0.25
 * /husky_velocity_controller/enable_odom_tf: False
 * /husky_velocity_controller/estimate_velocity_from_position: False
 * /husky_velocity_controller/left_wheel: ['front_left_whee...
 * /husky_velocity_controller/linear/x/has_acceleration_limits: True
 * /husky_velocity_controller/linear/x/has_velocity_limits: True
 * /husky_velocity_controller/linear/x/max_acceleration: 3.0
 * /husky_velocity_controller/linear/x/max_velocity: 1.0
 * /husky_velocity_controller/pose_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/publish_rate: 50
 * /husky_velocity_controller/right_wheel: ['front_right_whe...
 * /husky_velocity_controller/twist_covariance_diagonal: [0.001, 0.001, 0....
 * /husky_velocity_controller/type: diff_drive_contro...
 * /husky_velocity_controller/velocity_rolling_window_size: 2
 * /husky_velocity_controller/wheel_radius_multiplier: 1.0
 * /husky_velocity_controller/wheel_separation_multiplier: 1.875
 * /joy_teleop/joy_node/autorepeat_rate: 20
 * /joy_teleop/joy_node/deadzone: 0.1
 * /joy_teleop/joy_node/dev: /dev/input/ps4
 * /joy_teleop/teleop_twist_joy/axis_angular: 0
 * /joy_teleop/teleop_twist_joy/axis_linear: 1
 * /joy_teleop/teleop_twist_joy/enable_button: 4
 * /joy_teleop/teleop_twist_joy/enable_turbo_button: 5
 * /joy_teleop/teleop_twist_joy/scale_angular: 1.4
 * /joy_teleop/teleop_twist_joy/scale_linear: 0.4
 * /joy_teleop/teleop_twist_joy/scale_linear_turbo: 2.0
 * /robot_description: <?xml version="1....
 * /rosdistro: melodic
 * /rosversion: 1.14.13
 * /twist_mux/locks: [{'topic': 'e_sto...
 * /twist_mux/topics: [{'topic': 'joy_t...
 * /use_sim_time: True

NODES
  /
    base_controller_spawner (controller_manager/spawner)
    ekf_localization (robot_localization/ekf_localization_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_husky_model (gazebo_ros/spawn_model)
    twist_marker_server (interactive_marker_twist_server/marker_server)
    twist_mux (twist_mux/twist_mux)
  /joy_teleop/
    joy_node (joy/joy_node)
    teleop_twist_joy (teleop_twist_joy/teleop_node)

auto-starting new master
process[master]: started with pid [8716]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 450eef54-1ffa-11ed-aec0-080027d6af03
process[rosout-1]: started with pid [8741]
started core service [/rosout]
process[gazebo-2]: started with pid [8779]
process[gazebo_gui-3]: started with pid [8781]
process[base_controller_spawner-4]: started with pid [8786]
process[ekf_localization-5]: started with pid [8797]
process[twist_marker_server-6]: started with pid [8801]
process[robot_state_publisher-7]: started with pid [8806]
process[twist_mux-8]: started with pid [8809]
process[joy_teleop/joy_node-9]: started with pid [8812]
process[joy_teleop/teleop_twist_joy-10]: started with pid [8821]
process[spawn_husky_model-11]: started with pid [8836]
[ INFO] [1660939530.921030492]: [twist_marker_server] Initialized.
[ERROR] [1660939531.029559968]: Couldn't open joystick /dev/input/ps4. Will retry every second.
[ INFO] [1660939532.339201786]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1660939532.344237198]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1660939532.368953140]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1660939532.393044723]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1660939534.540928855]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[Err] [REST.cc:205] Error in REST request

libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'
[ WARN] [1660939535.307650170, 1298.480000000]: Failed to meet update rate! Took 1298.4800000000000182
[ERROR] [1660939535.320813, 1298.480000]: Spawn service failed. Exiting.
[ WARN] [1660939535.347604777, 1298.480000000]: Failed to meet update rate! Took 1298.4600000000000364
[ INFO] [1660939535.517826664, 1298.480000000]: Physics dynamic reconfigure ready.
[spawn_husky_model-11] process has died [pid 8836, exit code 1, cmd /opt/ros/melodic/lib/gazebo_ros/spawn_model -x 0.0 -y 0.0 -z 0.0 -Y 0.0 -unpause -urdf -param robot_description -model husky -robot_namespace / __name:=spawn_husky_model __log:=/home/administrator/.ros/log/450eef54-1ffa-11ed-aec0-080027d6af03/spawn_husky_model-11.log].
log file: /home/administrator/.ros/log/450eef54-1ffa-11ed-aec0-080027d6af03/spawn_husky_model-11*.log
[ INFO] [1660939535.828368900, 1298.480000000]: Loading gazebo_ros_control plugin
[ INFO] [1660939535.830541101, 1298.480000000]: Starting gazebo_ros_control plugin in namespace: /
[ INFO] [1660939535.847834859, 1298.480000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1660939536.010355249, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1660939536.037363580, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1660939536.068640007, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1660939536.089464244, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
[ INFO] [1660939536.166202200, 1298.480000000]: Loaded gazebo_ros_control.
[ INFO] [1660939536.177678538, 1298.480000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1660939536.190049961, 1298.480000000]: imu plugin missing <rpyOffset>, defaults to 0s
[ INFO] [1660939536.472896716, 1298.480000000]: Controller state will be published at 50Hz.
[ INFO] [1660939536.535195067, 1298.480000000]: Wheel separation will be multiplied by 1.875.
[ INFO] [1660939536.566514765, 1298.480000000]: Left wheel radius will be multiplied by 1.
[ INFO] [1660939536.566621550, 1298.480000000]: Right wheel radius will be multiplied by 1.
[ INFO] [1660939536.582144462, 1298.480000000]: Velocity rolling window size of 2.
[ INFO] [1660939536.609632910, 1298.480000000]: Velocity commands will be considered old if they are older than 0.25s.
[ INFO] [1660939536.813417250, 1298.480000000]: Allow mutiple cmd_vel publishers is enabled
[ INFO] [1660939536.833327937, 1298.480000000]: Base frame_id set to base_link
[ INFO] [1660939536.846101464, 1298.480000000]: Odometry frame_id set to odom
[ INFO] [1660939536.873636408, 1298.480000000]: Publishing to tf is disabled
[ INFO] [1660939537.200356001, 1298.480000000]: left wheel to origin: 0.256,0.2854, 0.03282
[ INFO] [1660939537.201876280, 1298.480000000]: right wheel to origin: 0.256,-0.2854, 0.03282
[ INFO] [1660939537.202134172, 1298.480000000]: Odometry params : wheel separation 1.07025, left wheel radius 0.1651, right wheel radius 0.1651
[ INFO] [1660939537.259424595, 1298.480000000]: Adding left wheel with joint name: front_left_wheel and right wheel with joint name: front_right_wheel
[ INFO] [1660939537.259660885, 1298.480000000]: Adding left wheel with joint name: rear_left_wheel and right wheel with joint name: rear_right_wheel
[ INFO] [1660939537.641359168, 1298.580000000]: Dynamic Reconfigure:
DynamicParams:
    Odometry parameters:
        left wheel radius multiplier: 1
        right wheel radius multiplier: 1
        wheel separation multiplier: 1.875
    Publication parameters:
        Publish executed velocity command: disabled
        Publication rate: 50
        Publish frame odom on tf: disabled

About a few of the errors and warnings:

[ERROR] [1660939531.029559968]: Couldn't open joystick /dev/input/ps4. Will retry every second.

This is normal; I don't have a PS4 controller connected, so the default teleop input will throw this warning. You can override the default controller with by setting

export HUSKY_LOGITECH=1
export HUSKY_JOY_DEVICE=/dev/input/js0  # substitute as appropriate
[ERROR] [1660939536.010355249, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1660939536.037363580, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1660939536.068640007, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1660939536.089464244, 1298.480000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel

This is a long-standing issue with Gazebo. The Husky's wheels do not use PID control, so no PID gains are specified. This shouldn't be reported as an error, but Gazebo does anyway.

tonybaltovski commented 2 years ago

Closing due to inactivity. Please feel free to re-open if issue persists.

w3ichen commented 5 months ago

UPDATE: I fixed this by building from source inside my UTM virtual machine, instead of using the binary from apt-get install.

  1. I built from source according to these instructions on the ign-math6 branch.
  2. Then replaced the .so file in /lib sudo cp at lib/x86_64-linux-gnu/libignition-math6.so.6 /lib/x86_64-linux-gnu/libignition-math6.so.6

    I'm getting an error of Thread 22 "gzserver" received signal SIGILL, Illegal instruction, when running gmapping_demo.launch.

System

Reproduction

  1. Run roslaunch husky_gazebo husky_playpen.launch, no errors at this point
  2. Run roslaunch husky_viz view_robot.launch will cause gazebo to error out

Error

Illegal instruction (core dumped)
[gazebo-2] process has died [pid 13647, exit code 132, cmd /opt/ros/noetic/lib/gazebo_ros/gzserver -e ode /home/husky
name: =gazebo
ot/src/mobile_manipulator/worlds/env.world __name:=gazebo - 1og:=/home/.ros/log/8ba903b4-242b-11ef-bc74-bbef25f09167/gazebo-2.log].
log file: /home/.ros/log/8ba903b4-242b-11ef-bc74-bbef25f09167/gazebo-2*.

GDB backtrace is showing gazebo math function is failing:

(gdb) bt

   #0  0x00007ffff60d6963 in double std::generate_canonical<double, 53ul, std::mersenne_twister_engine<unsigned long, 32ul, 624ul, 397ul, 31ul, 2567483615ul, 11ul, 4294967295ul, 7ul, 2636928640ul, 15ul, 4022730752ul, 18ul, 1812433253ul> >(std::mersenne_twister_engine<unsigned long, 32ul, 624ul, 397ul, 31ul, 2567483615ul, 11ul, 4294967295ul, 7ul, 2636928640ul, 15ul, 4022730752ul, 18ul, 1812433253ul>&)
    () at /lib/x86_64-linux-gnu/libignition-math6.so.6
#1  0x00007ffff60d6a04 in double std::normal_distribution<double>::operator()<std::mersenne_twister_engine<unsigned long, 32ul, 624ul, 397ul, 31ul, 2567483615ul, 11ul, 4294967295ul, 7ul, 2636928640ul, 15ul, 4022730752ul, 18ul, 1812433253ul> >(std::mersenne_twister_engine<unsigned long, 32ul, 624ul, 397ul, 31ul, 2567483615ul, 11ul, 4294967295ul, 7ul, 2636928640ul, 15ul, 4022730752ul, 18ul, 1812433253ul>&, std::normal_distribution<double>::param_type const&) ()
    at /lib/x86_64-linux-gnu/libignition-math6.so.6
#2  0x00007ffff60d6154 in ignition::math::v6::Rand::DblNormal(double, double)
    () at /lib/x86_64-linux-gnu/libignition-math6.so.6
#3  0x00007ffff72b16e7 in gazebo::sensors::GaussianNoiseModel::ApplyImpl(double, double) () at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#4  0x00007ffff72cdf71 in gazebo::sensors::Noise::Apply(double, double) ()
    at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#5  0x00007ffff72d1286 in gazebo::sensors::RaySensor::UpdateImpl(bool) ()
    at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#6  0x00007ffff72d59a9 in gazebo::sensors::Sensor::Update(bool) ()
    at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#7  0x00007ffff72e0f35 in gazebo::sensors::SensorManager::SensorContainer::Update(bool) () at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#8  0x00007ffff72e0797 in gazebo::sensors::SensorManager::SensorContainer::RunLoop() () at /lib/x86_64-linux-gnu/libgazebo_sensors.so.11
#9  0x00007ffff59b543b in  ()
    at /lib/x86_64-linux-gnu/libboost_thread.so.1.71.0
#10 0x00007ffff6f28609 in start_thread (arg=<optimized out>)
    at pthread_create.c:477
#11 0x00007ffff7464353 in clone ()
    at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95