RobotState "no msg received" but robot moving slightly while "Execute" is still running in Rviz
I'm running Ubuntu 20.04.3 LTS (Kernel: 5.14.13-surface x86_64) on a Microsoft Surface Book 2. My ros distro is noetic.
First of all I am still very new to ROS and have already learned a lot and fixes many issus myself but this one I cant figure out. I am trying to solve this issue for several weeks now and I don't make any progress... I am trying to build a custom 6DOF robot model controlled by Moveit via USB. I have successfully generated a URDF file using the fusion2urdf script for Fusion360. After that I used the moveitsetupassistant to create all the necessary files moveit needs to run. During my whole project I was mainly following a YouTube tutorial series from "stephen zuccaro" (Creating your Moveit Hardware Interface - C++) to create my hardware interface using the roscontrolboilerplate from PickNikRobotics. After a lot of trail and error I was finally able to successfully launch rviz. My robot shows up and I can use MotionPlanning to generate joint angles on the /joint_states topic. I have no errors during the startup of my main launch file except for this one:
[ERROR] [1634931694.555060870]: Tried to advertise on topic [/move_group/filtered_cloud] with md5sum [060021388200f6f0f447d0fcd9c64743] and datatype [sensor_msgs/Image], but the topic is already advertised as md5sum [1158d486dd51d683ce2f1be655c3c181] and datatype [sensor_msgs/PointCloud2]
The problem is that when I hit "Plan and Execute" the "grey" robot in Rviz is just moveing very slightly and stops as soon as the execution is "successfully" finished. So the next time I try to move the robot it starts calculating the joint angles based on the same starting pose as when rviz just launched. When I add "RobotState" to the "Displays" section it says: "No msg received". If i run
rosrun tf2_tools view_frames.py
I get a pdf file whis only says "no tf data recieved".
roswtf is giving me the following output:
Loaded plugin tf.tfwtf
No package or stack in the current directory
================================================================================
Static checks summary:
No errors or warnings
================================================================================
Beginning tests of your ROS graph. These may take a while...
analyzing graph...
... done analyzing graph
running graph rules...
... done running graph rules
running tf checks, this will take a second...
... tf checks complete
Online checks summary:
Found 2 warning(s).
Warnings are things that may be just fine, but are sometimes at fault
WARNING The following node subscriptions are unconnected:
* /robot_hw_main:
* /position_trajectory_controller/command
* /move_group:
* /collision_object
* /head_mount_kinect/depth_registered/points
* /head_mount_kinect/depth_registered/image_raw
* /head_mount_kinect/depth_registered/camera_info
* /sequence_move_group/goal
* /sequence_move_group/cancel
* /rviz:
* /recognized_object_array
* /display_robot_state
WARNING These nodes have died:
* ros_control_controller_manager-4
Can anyone explain to me why my robot is moving even tough /tf is not receiving anything?
Here is my launch file
<launch>
<!-- RVIZ -->
<!-- Load the URDF to the parameter server -->
<param name="robot_description" textfile="$(find roboturdf_description)/urdf/roboturdf.urdf"/>
<!-- Show in Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_control)/basic.rviz"/>
<!-- CORE -->
<!-- Load controller settings -->
<rosparam file="$(find robot_control)/cfg/robot_controllers.yaml" command="load"/>
<!-- Load hardware interface -->
<node name="robot_hw_main" pkg="robot_control" type="robot_hw_main" output="screen"/>
<!-- Load controller manager -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="spawn joint_state_controller position_trajectory_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
<!-- Simple robot simulator
<node name="robot_sim_echo" pkg="robot_control" type="robot_sim_echo"/> -->
<!-- MOVEIT -->
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find robot_description_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- Remap follow_joint_trajectory -->
<remap from="/joint_trajectory_action" to="/position_trajectory_controller/follow_joint_trajectory"/>
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find robot_description_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="false"/>
<arg name="info" value="true"/>
</include>
<!-- HARDWARE -->
<node pkg="rosserial_python" type="serial_node.py" name="serial_node">
<param name="port" value="/dev/ttyACM0"/>
<param name="baud" value="57600"/>
</node>
</launch>
And my robot_controllers.yaml file:
# ros_control_boilerplate Settings -----------------------
# Settings for ros_control control loop
generic_hw_control_loop:
loop_hz: 25
cycle_time_error_threshold: 0.1
# Settings for ros_control hardware interface (used in generic_hw_interface.cpp)
hardware_interface:
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
type: position_controllers/JointTrajectoryController
# These joints can likely just be copied from the hardware_interface list above
joints:
- joint1
- joint2
- joint3
- joint4
- joint5
- joint6
And my URDF file:
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from roboturdf.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="roboturdf">
<material name="silver">
<color rgba="0.700 0.700 0.700 1.000"/>
</material>
<transmission name="joint1_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint1_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint2_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint2_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint3_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint3">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint3_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint4_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint4">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint4_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint5_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint5">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint5_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="joint6_tran">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint6">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint6_actr">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="control"/>
</gazebo>
<gazebo reference="base_link">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
<gravity>true</gravity>
</gazebo>
<gazebo reference="axis1_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="axis2_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="axis3_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="axis4_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="axis5_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="toolMount_1">
<material>Gazebo/Silver</material>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<selfCollide>true</selfCollide>
</gazebo>
<link name="base_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.06922599698608356 7.211556563184012e-07 0.04276097300993153"/>
<mass value="0.3953963541600018"/>
<inertia ixx="0.001271" ixy="0.0" ixz="0.000669" iyy="0.003529" iyz="-0.0" izz="0.003758"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/base_link.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="axis1_1">
<inertial>
<origin rpy="0 0 0" xyz="-0.005684825319002044 0.0013779954429496513 -0.026576774212384566"/>
<mass value="0.4627358986440993"/>
<inertia ixx="0.000814" ixy="2e-06" ixz="-1.3e-05" iyy="0.001047" iyz="-1.9e-05" izz="0.00141"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0.0 -0.0 -0.109"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis1_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.0 -0.0 -0.109"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis1_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="axis2_1">
<inertial>
<origin rpy="0 0 0" xyz="-0.0006691091410762698 -0.051233857753293505 0.03400075967206073"/>
<mass value="0.17997047399058208"/>
<inertia ixx="0.000402" ixy="2e-06" ixz="3e-06" iyy="0.000278" iyz="-5.8e-05" izz="0.000199"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.047 -0.0494 -0.135"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis2_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.047 -0.0494 -0.135"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis2_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="axis3_1">
<inertial>
<origin rpy="0 0 0" xyz="-0.005494159339605349 -0.029095605471408773 -0.0012943168974930697"/>
<mass value="0.06351044258870171"/>
<inertia ixx="4.9e-05" ixy="-2e-06" ixz="-0.0" iyy="3.9e-05" iyz="-0.0" izz="2.7e-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.047 -0.0369 -0.245"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis3_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.047 -0.0369 -0.245"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis3_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="axis4_1">
<inertial>
<origin rpy="0 0 0" xyz="0.047470843955160345 -0.0004682923606764828 7.542907867863491e-06"/>
<mass value="0.05518369120051437"/>
<inertia ixx="1.5e-05" ixy="-0.0" ixz="0.0" iyy="5.7e-05" iyz="-0.0" izz="6.2e-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.056 0.0001 -0.271"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis4_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.056 0.0001 -0.271"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis4_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="axis5_1">
<inertial>
<origin rpy="0 0 0" xyz="0.0009814227645916496 0.019349637985491445 -0.0003313822265059718"/>
<mass value="0.015807092846444912"/>
<inertia ixx="3e-06" ixy="0.0" ixz="-0.0" iyy="3e-06" iyz="-0.0" izz="3e-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.1645 0.0171 -0.271"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis5_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.1645 0.0171 -0.271"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/axis5_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<link name="toolMount_1">
<inertial>
<origin rpy="0 0 0" xyz="-0.0011488829522366129 -8.925138731373516e-15 -0.000522671018627352"/>
<mass value="0.0007962850919950779"/>
<inertia ixx="0.0" ixy="0.0" ixz="-0.0" iyy="0.0" iyz="-0.0" izz="0.0"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="-0.1895 0.0001 -0.270952"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/toolMount_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.1895 0.0001 -0.270952"/>
<geometry>
<mesh filename="package://roboturdf_description/meshes/toolMount_1.stl" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="continuous">
<origin rpy="0 0 0" xyz="0.0 0.0 0.109"/>
<parent link="base_link"/>
<child link="axis1_1"/>
<axis xyz="0.0 -0.0 -1.0"/>
</joint>
<joint name="joint2" type="revolute">
<origin rpy="0 0 0" xyz="0.047 0.0494 0.026"/>
<parent link="axis1_1"/>
<child link="axis2_1"/>
<axis xyz="0.0 -1.0 -0.0"/>
<limit effort="100" lower="-2.356194" upper="1.396263" velocity="100"/>
</joint>
<joint name="joint3" type="revolute">
<origin rpy="0 0 0" xyz="0.0 -0.0125 0.11"/>
<parent link="axis2_1"/>
<child link="axis3_1"/>
<axis xyz="-0.0 -1.0 -0.0"/>
<limit effort="100" lower="-1.282817" upper="3.141593" velocity="100"/>
</joint>
<joint name="joint4" type="revolute">
<origin rpy="0 0 0" xyz="0.009 -0.037 0.026"/>
<parent link="axis3_1"/>
<child link="axis4_1"/>
<axis xyz="1.0 -0.0 -0.0"/>
<limit effort="100" lower="-4.712389" upper="1.570796" velocity="100"/>
</joint>
<joint name="joint5" type="revolute">
<origin rpy="0 0 0" xyz="0.1085 -0.017 0.0"/>
<parent link="axis4_1"/>
<child link="axis5_1"/>
<axis xyz="-0.0 -1.0 0.0"/>
<limit effort="100" lower="-2.356194" upper="2.356194" velocity="100"/>
</joint>
<joint name="joint6" type="continuous">
<origin rpy="0 0 0" xyz="0.025 0.017 -4.8e-05"/>
<parent link="axis5_1"/>
<child link="toolMount_1"/>
<axis xyz="-1.0 0.0 -0.0"/>
</joint>
</robot>
Here is the YouTube Video showing the Problem If you need any other files or outputs let me know. Thanks in advance!
Asked by Josh98 on 2021-10-22 15:58:13 UTC
Comments