search

TechmanRobotInc / tmr_ros2

TM Robots supporting ROS2 drivers and some extended external applications. (experimental) (not support the new TM S-Series)
Other
37 stars 21 forks source link

Goal was rejected by server: i can't move a TM5-900 #29

Closed gaudfra closed 1 year ago

gaudfra commented 1 year ago

Hi there, i am currently trying to move a real TM5-900 with tm_driver, but i keep getting the following errors:

This is my terminal: ros2 launch project tm_launch.py robot_ip:=192.168.1.123 [INFO] [launch]: All log files can be found below /home/fragaud/.ros/log/2023-03-10-14-02-23-919105-fg-191512 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [tm_driver-1]: process started with pid [191514] [INFO] [static_transform_publisher-2]: process started with pid [191516] [INFO] [robot_state_publisher-3]: process started with pid [191518] [INFO] [project-4]: process started with pid [191520] [INFO] [move_group-5]: process started with pid [191522] [tm_driver-1] [INFO] [1678453344.179561321] [rclcpp]: STAMPA INIZIALE [tm_driver-1] [INFO] [1678453344.179737124] [rclcpp]: DENTRO TM_ROBOT_STATE [tm_driver-1] [INFO] [1678453344.179745937] [rclcpp]: PROVAEthernet slave communication: TmSvrCommunication [tm_driver-1] [INFO] [1678453344.179749739] [rclcpp]: TM_ROS: hello dentro TmSvrComm! [tm_driver-1] [INFO] [1678453344.179757899] [rclcpp]: Listen node communication: TmSctCommunication [tm_driver-1] [INFO] [1678453344.179761658] [rclcpp]: HELLOO dentro composition moveit! [static_transform_publisher-2] [INFO] [1678453344.192755005] [static_transform_publisher]: Spinning until killed publishing transform from 'world' to 'base' [tm_driver-1] [INFO] [1678453344.198767271] [rclcpp]: DENTRO TMSVRROS2 [tm_driver-1] [INFO] [1678453344.198795859] [rclcpp]: verifica start_tm_svr [tm_driver-1] [INFO] [1678453344.198803376] [rclcpp]: HA FATTO L'HALT [tm_driver-1] [INFO] [1678453344.198809508] [rclcpp]: HELLOOOOOOEthernet slave communication: start [tm_driver-1] [INFO] [1678453344.198815785] [rclcpp]: DENTRO CONNECT_SOCKET [tm_driver-1] [INFO] [1678453344.198835770] [rclcpp]: TM_COM: ip:=192.168.1.123 [tm_driver-1] [INFO] [1678453344.201226591] [rclcpp]: TM_COM(Ethernet slave communication): TM robot is connected. sockfd:=16 [tm_driver-1] [INFO] [1678453344.201350548] [rclcpp]: TM_ROS: get data thread begin [robot_state_publisher-3] Parsing robot urdf xml string. [robot_state_publisher-3] Link link_0 had 1 children [robot_state_publisher-3] Link link_1 had 1 children [robot_state_publisher-3] Link link_2 had 1 children [robot_state_publisher-3] Link link_3 had 1 children [robot_state_publisher-3] Link link_4 had 1 children [robot_state_publisher-3] Link link_5 had 1 children [robot_state_publisher-3] Link link_6 had 1 children [robot_state_publisher-3] Link flange had 0 children [robot_state_publisher-3] [INFO] [1678453344.203165746] [robot_state_publisher]: got segment base [robot_state_publisher-3] [INFO] [1678453344.203246303] [robot_state_publisher]: got segment flange [robot_state_publisher-3] [INFO] [1678453344.203254967] [robot_state_publisher]: got segment link_0 [robot_state_publisher-3] [INFO] [1678453344.203260882] [robot_state_publisher]: got segment link_1 [robot_state_publisher-3] [INFO] [1678453344.203267481] [robot_state_publisher]: got segment link_2 [robot_state_publisher-3] [INFO] [1678453344.203274428] [robot_state_publisher]: got segment link_3 [robot_state_publisher-3] [INFO] [1678453344.203280739] [robot_state_publisher]: got segment link_4 [robot_state_publisher-3] [INFO] [1678453344.203287316] [robot_state_publisher]: got segment link_5 [robot_state_publisher-3] [INFO] [1678453344.203293615] [robot_state_publisher]: got segment link_6 [tm_driver-1] [INFO] [1678453344.203373883] [rclcpp]: Listen node communication: start [tm_driver-1] [INFO] [1678453344.203389095] [rclcpp]: DENTRO CONNECT_SOCKET [tm_driver-1] [INFO] [1678453344.203710967] [rclcpp]: TM_COM(listen node communication): TM robot is connected. sockfd:=17 [tm_driver-1] [INFO] [1678453344.211597428] [rclcpp]: STAMPA DENTRO TMROS2MOVIT [project-4] ROS node creation! [project-4] [INFO] [1678453344.228380140] [project]: MoveNode created successfully [project-4] MOVE GROUP INTERFACE [move_group-5] [WARN] [1678453344.230471894] [move_group.move_group]: MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default planning pipeline configuration [move_group-5] [WARN] [1678453344.230763883] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior). [move_group-5] Parsing robot urdf xml string. [move_group-5] [INFO] [1678453344.235010853] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.000944667 seconds [move_group-5] [INFO] [1678453344.235047305] [moveit_robot_model.robot_model]: Loading robot model 'tm5-900'... [project-4] Parsing robot urdf xml string. [project-4] [INFO] [1678453344.236921864] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.000806259 seconds [project-4] [INFO] [1678453344.236948390] [moveit_robot_model.robot_model]: Loading robot model 'tm5-900'... [tm_driver-1] [INFO] [1678453344.248305248] [rclcpp]: DENTRO deserialize_first_time [tm_driver-1] [INFO] [1678453344.248321855] [rclcpp]: TM Flow DataTable Checked Item: [tm_driver-1] [INFO] [1678453344.248377864] [rclcpp]: Total 47 item,47 checked, 0 skipped [tm_driver-1] [INFO] [1678453344.248390462] [rclcpp]: data table is correct! [tm_driver-1] [INFO] [1678453344.253864774] [rclcpp]: TM_ROS: sct_response thread begin [move_group-5] Link link_0 had 1 children [move_group-5] Link link_1 had 1 children [move_group-5] Link link_2 had 1 children [move_group-5] Link link_3 had 1 children [move_group-5] Link link_4 had 1 children [move_group-5] Link link_5 had 1 children [move_group-5] Link link_6 had 1 children [move_group-5] Link flange had 0 children [project-4] Link link_0 had 1 children [project-4] Link link_1 had 1 children [project-4] Link link_2 had 1 children [project-4] Link link_3 had 1 children [project-4] Link link_4 had 1 children [project-4] Link link_5 had 1 children [project-4] Link link_6 had 1 children [project-4] Link flange had 0 children

[tm_driver-1] [tm_driver-1] [INFO] [1678453344.280041394] [rclcpp]: TM_ROS: (TM_STA): res: (00): true,Listen1 [tm_driver-1] [INFO] [1678453344.280091532] [rclcpp]: TM_ROS: On listen node. [move_group-5] [INFO] [1678453344.301225956] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' [move_group-5] [INFO] [1678453344.301352830] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states [move_group-5] [INFO] [1678453344.301663778] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group-5] [INFO] [1678453344.301915815] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects [move_group-5] [INFO] [1678453344.301928860] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor [move_group-5] [INFO] [1678453344.302128188] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene' [move_group-5] [INFO] [1678453344.302139222] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [move_group-5] [INFO] [1678453344.302355210] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object' [move_group-5] [INFO] [1678453344.302590198] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry [move_group-5] [WARN] [1678453344.302621321] [moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead [move_group-5] [ERROR] [1678453344.302648663] [moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates [move_group-5] [INFO] [1678453344.323494232] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group' [move_group-5] [INFO] [1678453344.324705336] [moveit.ros_planning.planning_pipeline]: No '~planning_plugin' parameter specified, but only 'ompl_interface/OMPLPlanner' planning plugin is available. Using that one. [move_group-5] [INFO] [1678453344.342327573] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL' [move_group-5] [INFO] [1678453344.360893011] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for tmr_arm_controller [move_group-5] [INFO] [1678453344.361035747] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list [move_group-5] [INFO] [1678453344.361380667] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers [move_group-5] [INFO] [1678453344.361394613] [move_group.move_group]: MoveGroup debug mode is ON [project-4] [INFO] [1678453344.373257050] [move_group_interface]: Ready to take commands for planning group tmr_arm. [project-4] Plan creation [move_group-5] [INFO] [1678453344.373575369] [move_group.move_group]: [move_group-5] [move_group-5] **** [move_group-5] MoveGroup using: [move_group-5] - ApplyPlanningSceneService [move_group-5] - ClearOctomapService [move_group-5] - CartesianPathService [move_group-5] - ExecuteTrajectoryAction [move_group-5] - GetPlanningSceneService [move_group-5] - KinematicsService [move_group-5] - MoveAction [move_group-5] - MotionPlanService [move_group-5] - QueryPlannersService [move_group-5] * - StateValidationService [move_group-5] **** [move_group-5] [move_group-5] [INFO] [1678453344.373610054] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner [move_group-5] [INFO] [1678453344.373620670] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete [move_group-5] Loading 'move_group/ApplyPlanningSceneService'... [move_group-5] Loading 'move_group/ClearOctomapService'... [move_group-5] Loading 'move_group/MoveGroupCartesianPathService'... [move_group-5] Loading 'move_group/MoveGroupExecuteTrajectoryAction'... [move_group-5] Loading 'move_group/MoveGroupGetPlanningSceneService'... [move_group-5] Loading 'move_group/MoveGroupKinematicsService'... [move_group-5] Loading 'move_group/MoveGroupMoveAction'... [move_group-5] Loading 'move_group/MoveGroupPlanService'... [move_group-5] Loading 'move_group/MoveGroupQueryPlannersService'... [move_group-5] Loading 'move_group/MoveGroupStateValidationService'... [move_group-5] [move_group-5] You can start planning now! [move_group-5] [move_group-5] [INFO] [1678453346.375494402] [moveit_move_group_default_capabilities.move_action_capability]: Received request

[move_group-5] [INFO] [1678453346.377345868] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [move_group-5] [WARN] [1678453346.378906613] [moveit.ompl_planning.model_based_planning_context]: It looks like the planning volume was not specified. [move_group-5] [INFO] [1678453346.380019296] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'tmr_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-5] [INFO] [1678453346.381194797] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - tmr_arm/tmr_arm: Starting planning with 1 states already in datastructure [move_group-5] [INFO] [1678453346.394971895] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - tmr_arm/tmr_arm: Created 4 states (2 start + 2 goal) [move_group-5] [INFO] [1678453346.395034835] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:138 - Solution found in 0.014252 seconds [move_group-5] [INFO] [1678453346.403189089] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:179 - SimpleSetup: Path simplification took 0.008008 seconds and changed from 3 to 2 states [move_group-5] [INFO] [1678453346.404091324] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed succesfully. [project-4] [INFO] [1678453346.776774176] [move_group_interface]: Planning request accepted [project-4] [INFO] [1678453346.877948537] [move_group_interface]: Planning request complete! [project-4] [INFO] [1678453346.978180382] [move_group_interface]: time taken to generate plan: 0.0222607 seconds [project-4] [INFO] [1678453346.978221112] [project]: Visualizing plan 1 (pose goal) [project-4] success done [project-4] Prima di move [move_group-5] [INFO] [1678453346.978412392] [moveit_move_group_default_capabilities.move_action_capability]: Received request

[move_group-5] [INFO] [1678453346.991918173] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [move_group-5] [INFO] [1678453346.992091798] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1 [move_group-5] [WARN] [1678453346.992278747] [moveit.ompl_planning.model_based_planning_context]: It looks like the planning volume was not specified. [move_group-5] [INFO] [1678453346.992480295] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'tmr_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-5] [INFO] [1678453346.992693058] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - tmr_arm/tmr_arm: Starting planning with 1 states already in datastructure [move_group-5] [INFO] [1678453347.004558395] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - tmr_arm/tmr_arm: Created 4 states (2 start + 2 goal) [move_group-5] [INFO] [1678453347.004604415] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:138 - Solution found in 0.011989 seconds [move_group-5] [INFO] [1678453347.008509434] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:179 - SimpleSetup: Path simplification took 0.003864 seconds and changed from 3 to 2 states [move_group-5] [ERROR] [1678453347.009024611] [moveit_ros.planning_scene_monitor.trajectory_monitor]: The sampling frequency for trajectory states should be positive [move_group-5] [INFO] [1678453347.009086829] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01 [move_group-5] [INFO] [1678453347.022179245] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ... [move_group-5] [INFO] [1678453347.022355262] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to tmr_arm_controller [tm_driver-1] [INFO] [1678453347.023176959] [rclcpp]: Received new action goal 37147cfbbc91bee9c5b5ec1eeb70aa [move_group-5] [INFO] [1678453347.023722805] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: tmr_arm_controller started execution [move_group-5] [WARN] [1678453347.023772962] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request rejected [move_group-5] [ERROR] [1678453347.023775548] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal was rejected by server [move_group-5] [ERROR] [1678453347.023849680] [moveit_ros.trajectory_execution_manager]: Failed to send trajectory part 1 of 1 to controller tmr_arm_controller [move_group-5] [INFO] [1678453347.023881506] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ... [move_group-5] [INFO] [1678453347.032365196] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution [project-4] [INFO] [1678453347.379345264] [move_group_interface]: Plan and Execute request accepted [project-4] [INFO] [1678453347.479737328] [move_group_interface]: Plan and Execute request aborted [project-4] [ERROR] [1678453347.579933444] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached [project-4] terminate called after throwing an instance of 'std::runtime_error' [project-4] After move [project-4] what(): Node has already been added to an executor. [ERROR] [project-4]: process has died [pid 191520, exit code -6, cmd '/home/fragaud/tm_ws/install/project/lib/project/project --ros-args --params-file /tmp/launch_params_068dewg8 --params-file /tmp/launch_params_h9riuq3e --params-file /tmp/launch_params_r3agflm5 --params-file /tmp/launch_params_r1qf_s4g --params-file /tmp/launch_params_via4beh7']. [tm_driver-1] [WARN] [1678453355.120460958] [rclcpp]: package length not valid ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)

My goal is to simply move the robot to a specified position, with the move() method of the move group interface. This is the code i'm using: `#include <moveit/move_group_interface/move_group_interface.h>

include <moveit/planning_scene_interface/planning_scene_interface.h>

include <moveit_msgs/msg/display_robot_state.hpp>

include <moveit_msgs/msg/display_trajectory.hpp>

include <moveit_msgs/msg/attached_collision_object.hpp>

include <moveit_msgs/msg/collision_object.hpp>

include "rclcpp/rclcpp.hpp"

include "geometry_msgs/msg/pose_stamped.hpp"

using namespace std;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("project"); int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); cout << "ROS node creation!" << endl; auto move_node = rclcpp::Node::make_shared("project", node_options); RCLCPP_INFO(move_node->get_logger(), "MoveNode created successfully");

rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_node); std::thread([&executor]() { executor.spin(); }).detach(); static const string PLANNING_GROUP_ARM = "tmr_arm"; static const string PLANNING_GROUP_GRIPPER = "gripper"; cout << "MOVE GROUP INTERFACE" << endl;

moveit::planning_interface::MoveGroupInterface move_group_interface_arm(move_node, PLANNING_GROUP_ARM);

cout << "Plan creation" << endl; rclcpp::sleep_for(std::chrono::seconds(2)); moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;

// 1. Move to first position move_group_interface_arm.setJointValueTarget(move_group_interface_arm.getNamedTargetValues("ready1"));

bool success = (move_group_interface_arm.plan(my_plan_arm) == moveit::planning_interface::MoveItErrorCode::SUCCESS); cout << "success done" << endl; RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); cout << "Prima di move" << endl; move_group_interface_arm.move(); cout << "After move" << endl; rclcpp::spin(move_node); rclcpp::shutdown(); return 0; }`