search

AlessioTonioni / Autonomous-Flight-ROS

A simple autopilot for a quadrotor realized using MoveIt!. The system use a simulated RGBD sensor to reconstruct the map, then ompl for path generation.
47 stars 24 forks source link

Failed to validate trajectory: couldn't receive full current joint state within 1s #7

Open ghost opened 7 years ago

ghost commented 7 years ago

Hello! Currently, I have no problem planning a motion but the problem happens when i execute my planning from moveit to gazebo, it gives the error: "Failed to validate trajectory: couldn't receive full current joint state within 1s".

I'm currently using ros indigo, gazebo 2.2.6, moveit last update (Feb 2017), ubuntu 14.04 trusty.

In the terminal, when i try to plan and execute a valid state, the screenshot is attached in the link below https://drive.google.com/file/d/0B0_mv7dnS9ezbzNQUHlRWlc5Tlk/view?usp=sharing

The rqt_graph is attached in the link below. https://drive.google.com/file/d/0B0_mv7dnS9ezVnpTVUs1YVVJMk0/view?usp=sharing

I will be grateful if you could help me on this problem. Thanks you..

AlessioTonioni commented 7 years ago

"Failed to validate trajectory: couldn't receive full current joint state within 1s".

In your urdf there should be the definition for a plugin that will publish the joint state every x times per second (in my setup I had only one joint corresponding to the position of the drone itself). It seems that right now your model is not publishing this information or the node that should move your drone is not receiving it.

Try to do an echo of the joint state topic and check that everything is published well.

ghost commented 7 years ago

Hi @AlessioTonioni, Thanks for the reply. I have tried $echo rostopic /joint_states, but nothing appear. What should i do to solve this problem?

AlessioTonioni commented 7 years ago

Try to echo /tf, if I remember well joint states should be published there.

If they are not check your urdf, there should be somewhere the declaration of a plugin to publish the joint state

ghost commented 7 years ago

@AlessioTonioni Thanks for the help! I installed the plugin by running sudo apt-get install ros-indigo-pr2-gazebo-plugins Everything seems to published well.

SUMMARY
========

PARAMETERS
 * /gazebo/quadrotor_aerodynamics/C_mxy: 0.074156208
 * /gazebo/quadrotor_aerodynamics/C_mz: 0.050643264
 * /gazebo/quadrotor_aerodynamics/C_wxy: 0.12
 * /gazebo/quadrotor_aerodynamics/C_wz: 0.1
 * /gazebo/quadrotor_propulsion/CT0s: 1.53819048398e-05
 * /gazebo/quadrotor_propulsion/CT1s: -0.00025224
 * /gazebo/quadrotor_propulsion/CT2s: -0.013077
 * /gazebo/quadrotor_propulsion/J_M: 2.5730480633e-05
 * /gazebo/quadrotor_propulsion/Psi: 0.00724217982751
 * /gazebo/quadrotor_propulsion/R_A: 0.201084219222
 * /gazebo/quadrotor_propulsion/alpha_m: 0.104863758314
 * /gazebo/quadrotor_propulsion/beta_m: 0.549262344778
 * /gazebo/quadrotor_propulsion/k_m: -7.01163190977e-05
 * /gazebo/quadrotor_propulsion/k_t: 0.0153368647144
 * /gazebo/quadrotor_propulsion/l_m: 0.275
 * /ground_truth_to_tf/frame_id: /odom_combined
 * /ground_truth_to_tf/odometry_topic: ground_truth/state
 * /ground_truth_to_tf/tf_prefix: 
 * /robot_description: <?xml version="1....
 * /robot_state_publisher/publish_frequency: 50.0
 * /robot_state_publisher/tf_prefix: 
 * /rosdistro: indigo
 * /rosversion: 1.11.20
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    ground_truth_to_tf (message_to_tf/message_to_tf)
    robot_state_publisher (robot_state_publisher/robot_state_publisher)
    spawn_robot (gazebo_ros/spawn_model)

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

setting /run_id to edf281ec-f9c9-11e6-bd89-080027dd0fd0
process[rosout-1]: started with pid [2399]
started core service [/rosout]
process[gazebo-2]: started with pid [2402]
process[gazebo_gui-3]: started with pid [2405]
process[spawn_robot-4]: started with pid [2410]
process[robot_state_publisher-5]: started with pid [2411]
process[ground_truth_to_tf-6]: started with pid [2418]
[ WARN] [1487855639.677501013]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Gazebo multi-robot simulator, version 2.2.3
Copyright (C) 2012-2014 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

Msg Waiting for master
Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.2.15
[ INFO] [1487855644.385382874]: Finished loading Gazebo ROS API Plugin.
Msg Waiting for master[ INFO] [1487855644.387371023]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...

Msg Connected to gazebo master @ http://127.0.0.1:11345
Msg Publicized address: 10.0.2.15
spawn_model script started
Warning [gazebo.cc:215] Waited 1seconds for namespaces.
[INFO] [WallTime: 1487855645.412494] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1487855645.415669] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[ INFO] [1487855645.996512805, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[INFO] [WallTime: 1487855646.058533] [0.068000] Calling service /gazebo/spawn_urdf_model
[ INFO] [1487855647.403866596, 0.164000000]: Camera Plugin (robotNamespace = /), Info: Using the 'robotNamespace' param: '/'
[INFO] [WallTime: 1487855647.412400] [0.164000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1487855647.452997470, 0.164000000]: Physics dynamic reconfigure ready.
[ INFO] [1487855647.606641979, 0.164000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ WARN] [1487855647.606688036, 0.164000000]: dynamic reconfigure is not enabled for this image topic [camera/rgb/image_raw] becuase <cameraName> is not specified
[spawn_robot-4] process has finished cleanly
log file: /home/wenxin/.ros/log/edf281ec-f9c9-11e6-bd89-080027dd0fd0/spawn_robot-4*.log
[ INFO] [1487855648.790238021, 0.164000000]: imu plugin missing <xyzOffset>, defaults to 0s
[ INFO] [1487855648.790351191, 0.164000000]: imu plugin missing <rpyOffset>, defaults to 0s
Dbg plugin model name: quadrotor
[ INFO] [1487855649.512176349, 0.164000000]: starting gazebo_ros_controller_manager plugin in ns: /
[ INFO] [1487855649.512478691, 0.164000000]: Callback thread id=7f9319294700
[ INFO] [1487855649.520354498, 0.164000000]: gazebo controller manager plugin is waiting for urdf: //robot_description on the param server.  (make sure there is a rosparam by that name in the ros parameter server, otherwise, this plugin blocks simulation forever).
[ INFO] [1487855649.625644032, 0.164000000]: gazebo controller manager got pr2.xml from param server, parsing it...
[ WARN] [1487855649.643582950, 0.164000000]: No transmissions were specified in the robot description.
[ WARN] [1487855649.643735499, 0.164000000]: None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.
[ WARN] [1487855649.694034734, 0.164000000]: No transmissions were specified in the robot description.
[ WARN] [1487855649.694185685, 0.164000000]: None of the joints in the robot desription matches up to a motor. The robot is uncontrollable.
Warning [gazebo_quadrotor_simple_controller.cpp:57] The GazeboQuadrotorSimpleController plugin is DEPRECATED in ROS hydro. Please use the twist_controller in package hector_quadrotor_controller instead.
[ INFO] [1487855649.754658367, 0.164000000]: Using imu information on topic raw_imu as source of orientation and angular velocity.
[ INFO] [1487855649.772674508, 0.164000000]: Using state information on topic ground_truth/state as source of state information.
Warning [Publisher.cc:134] Queue limit reached for topic /gazebo/default/pose/local/info, deleting message. This warning is printed only once.

I tried to execute my planned path in Rviz again after installing the plugin, but Rviz crashes when executing the path, part of the code shown in the terminal: 

[ INFO] [1487855692.797447183, 26.796000000]: Loading robot model 'quadrotor'...
[ INFO] [1487855693.286792841, 26.950000000]: Loading robot model 'quadrotor'...
[ERROR] [1487855693.291520666, 26.951000000]: Group 'Quadrotore' is not a chain
[ERROR] [1487855693.291688754, 26.951000000]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'Quadrotore'
[ERROR] [1487855693.292590148, 26.951000000]: Kinematics solver could not be instantiated for joint group Quadrotore.
[ INFO] [1487855693.824288116, 27.100000000]: Starting scene monitor
[ INFO] [1487855693.836435347, 27.106000000]: Listening to '/move_group/monitored_planning_scene'
[ INFO] [1487855694.588071452, 27.363000000]: Constructing new MoveGroup connection for group 'Quadrotore' in namespace ''
[ INFO] [1487855695.735043504, 27.688000000]: TrajectoryExecution will use old service capability.
[ INFO] [1487855695.735162971, 27.688000000]: Ready to take MoveGroup commands for group Quadrotore.
[ INFO] [1487855695.735217112, 27.688000000]: Looking around: no
[ INFO] [1487855695.735324141, 27.688000000]: Replanning: no
[ INFO] [1487855749.096688163, 46.088000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1487855749.128974864, 46.105000000]: No optimization objective specified, defaulting to PathLengthOptimizationObjective
[ INFO] [1487855749.144258497, 46.105000000]: Planner configuration 'Quadrotore[PRMstarkConfigDefault]' will use planner 'geometric::PRMstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1487855749.149615169, 46.105000000]: Quadrotore[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1487855749.151302811, 46.105000000]: Quadrotore[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1487855749.151530099, 46.105000000]: Quadrotore[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1487855749.151656590, 46.105000000]: Quadrotore[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1487855749.151771946, 46.105000000]: Quadrotore[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ INFO] [1487855749.165545655, 46.112000000]: Quadrotore[PRMstarkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1487855749.173499560, 46.115000000]: Quadrotore[PRMstarkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1487855749.174780536, 46.115000000]: Quadrotore[PRMstarkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1487855749.181168265, 46.117000000]: Quadrotore[PRMstarkConfigDefault]: Starting planning with 2 states already in datastructure
[ INFO] [1487855754.152703397, 47.806000000]: Quadrotore[PRMstarkConfigDefault]: Created 1058 states
[ INFO] [1487855754.172197774, 47.812000000]: Quadrotore[PRMstarkConfigDefault]: Created 803 states
[ INFO] [1487855754.178703577, 47.814000000]: Quadrotore[PRMstarkConfigDefault]: Created 773 states
[ INFO] [1487855754.178875221, 47.814000000]: Quadrotore[PRMstarkConfigDefault]: Created 867 states
[ INFO] [1487855754.185287147, 47.817000000]: ParallelPlan::solve(): Solution found by one or more threads in 5.033372 seconds
[ INFO] [1487855801.224731980, 65.203000000]: Received new trajectory execution service request...
[move_group-1] process has died [pid 3299, exit code -11, cmd /opt/ros/indigo/lib/moveit_ros_move_group/move_group __name:=move_group __log:=/home/wenxin/.ros/log/edf281ec-f9c9-11e6-bd89-080027dd0fd0/move_group-1.log].
log file: /home/wenxin/.ros/log/edf281ec-f9c9-11e6-bd89-080027dd0fd0/move_group-1*.log

Sorry for the long post, as i have no clue what could be wrong for this situation. Any help will be greatly appreciated. Thanks!

fatihksubasi commented 7 years ago

@wenxin615 could you solve the problem. I have the same error here!

RyanDirgantara commented 6 years ago

Hi guys did you solve the problem? I'm having the same problem here

Did you manage to publish the joint_state?

AlessioTonioni commented 6 years ago

For future reference to expand on my previous answers, in my setup joint state are published by a libgazebo plugin defined inside the urdf file.

The plugin definition is defined between line and 134 and 140 on quadrotore.urdf

tahsinkose commented 6 years ago

@AlessioTonioni , I have tried your setup. Nothing is published on /joint_states. The plugin you refer only applies a transformation between the body and the world frames on the specified topic in the form of nav_msgs/Odometry. What we need is a message in the form of sensor_msgs/JointState that MoveIt! fetches from a proper topic so that it can validate the current state of robot.

If you really have an output on rostopic echo /joint_states, could you post a screenshot in here? Or a full walkthrough on the steps needed?

tahsinkose commented 6 years ago

Well, I can fetch the joint state - in this case simple Pose of drone - from various topics and services. My desire is to realize this through /joint_states topic, not some /joint_state topic. Also the plugin you are referring is gazebo_ros_p3d. There is nothing related with /joint_states in it.

AlessioTonioni commented 6 years ago

Unfortunately, I have not used this code in the last 4.5 year so I am not really able to test anything.

My best advice is to follow the far better step by step tutorial by will selby.

tahsinkose commented 6 years ago

For the granularity of the issue, I have found my way directed to here. Will Selby had done his simulation with Rotors drone, whereas yours is with Hector. I will try to replicate his work until I find a clue related to /joint_states, which is essential for MoveIt! to work. Thanks for the help anyways.