diff --git a/CMakeLists.txt b/CMakeLists.txt index 52fbc61977967f91efb429a7ccf75459b52eff12..2cb135561dd7daa882ea134a9d94c06db50e5553 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -210,9 +210,9 @@ add_executable(worldsafety ## Specify libraries to link a library or executable target against -target_link_libraries(robot_models_node ${catkin_LIBRARIES} ) +##target_link_libraries(robot_models_node ${catkin_LIBRARIES} ) target_link_libraries(worldsafety ${catkin_LIBRARIES} ) -target_link_libraries(safetytest ${catkin_LIBRARIES} ) +##target_link_libraries(safetytest ${catkin_LIBRARIES} ) #add_dependencies(robot_models_node ${catkin_LIBRARIES}) ############# diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make index fe0e1d7c6e17f8d858ed5472a9a836b04db16370..d37e2bdb8d9fcd97a2960011879443b69e3e3eab 100644 --- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make +++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/build.make @@ -79,106 +79,6 @@ robot_models_node_EXTERNAL_OBJECTS = devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/build.make -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_visual_tools.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_gui.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libeigen_conversions.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf_conversions.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_conversions.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_interface.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_cpp.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_warehouse.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libwarehouse_ros.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_pick_place_planner.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_rdf_loader.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model_loader.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_pipeline.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_plan_execution.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_exceptions.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_background_processing.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_base.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_model.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_transforms.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_state.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_robot_trajectory.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_interface.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_scene.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_constraint_samplers.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_profiler.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_trajectory_processing.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_distance_field.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_collision_distance_field.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_dynamics_solver.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_utils.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmoveit_test_utils.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_iostreams.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libfcl.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libkdl_parser.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liburdf.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_sensor.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model_state.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_model.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liburdfdom_world.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_bridge.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libsrdfdom.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libgeometric_shapes.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomap.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liboctomath.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librandom_numbers.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libclass_loader.so -devel/lib/robot_models/robot_models_node: /usr/lib/libPocoFoundation.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libdl.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroslib.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librospack.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpython2.7.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_program_options.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libtinyxml2.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2_ros.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libactionlib.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libmessage_filters.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_log4cxx.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librosconsole_backend_interface.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/liblog4cxx.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_regex.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libxmlrpcpp.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libtf2.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libroscpp_serialization.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/librostime.so -devel/lib/robot_models/robot_models_node: /opt/ros/melodic/lib/libcpp_common.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_system.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_thread.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_chrono.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_date_time.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libboost_atomic.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libpthread.so -devel/lib/robot_models/robot_models_node: /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 devel/lib/robot_models/robot_models_node: CMakeFiles/robot_models_node.dir/link.txt @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable devel/lib/robot_models/robot_models_node" $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/robot_models_node.dir/link.txt --verbose=$(VERBOSE) diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt index ec80df68feb54f8e140ee7c70a87d69f8fd0c1fa..12ee203c2f4cbced72583ffbd879597e95df4b30 100644 --- a/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt +++ b/cmake-build-debug/CMakeFiles/robot_models_node.dir/link.txt @@ -1 +1 @@ -/usr/bin/c++ -g -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o -o devel/lib/robot_models/robot_models_node -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 +/usr/bin/c++ -g -rdynamic CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o -o devel/lib/robot_models/robot_models_node diff --git a/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o b/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o deleted file mode 100644 index 21d078b22e17018fb55ae046320efc15639d3c2f..0000000000000000000000000000000000000000 Binary files a/cmake-build-debug/CMakeFiles/robot_models_node.dir/src/robot_models_node.cpp.o and /dev/null differ diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/build.make b/cmake-build-debug/CMakeFiles/safetytest.dir/build.make index 63d5ee2915bcc7d9a35a665d3686059cb7fbeeb1..e93150b4e02115841deb10b74a57bf86fb66db09 100644 --- a/cmake-build-debug/CMakeFiles/safetytest.dir/build.make +++ b/cmake-build-debug/CMakeFiles/safetytest.dir/build.make @@ -79,106 +79,6 @@ safetytest_EXTERNAL_OBJECTS = devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/src/safetytest.cpp.o devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/build.make -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_visual_tools.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_gui.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libeigen_conversions.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf_conversions.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libkdl_conversions.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_move_group_interface.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_cpp.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_warehouse.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libwarehouse_ros.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_pick_place_planner.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_rdf_loader.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_model_loader.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_pipeline.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_plan_execution.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_exceptions.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_background_processing.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_base.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_model.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_transforms.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_state.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_robot_trajectory.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_interface.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_detection.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_scene.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_constraint_samplers.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_profiler.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_trajectory_processing.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_distance_field.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_collision_distance_field.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_dynamics_solver.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_utils.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmoveit_test_utils.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_iostreams.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libfcl.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libkdl_parser.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liburdf.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_sensor.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_model_state.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_model.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liburdfdom_world.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_bridge.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libsrdfdom.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libtinyxml.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libgeometric_shapes.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liboctomap.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liboctomath.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librandom_numbers.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libclass_loader.so -devel/lib/robot_models/safetytest: /usr/lib/libPocoFoundation.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libdl.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroslib.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librospack.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libpython2.7.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_program_options.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libtinyxml2.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liborocos-kdl.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf2_ros.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libactionlib.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libmessage_filters.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroscpp.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_log4cxx.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librosconsole_backend_interface.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/liblog4cxx.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_regex.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libxmlrpcpp.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libtf2.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libroscpp_serialization.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/librostime.so -devel/lib/robot_models/safetytest: /opt/ros/melodic/lib/libcpp_common.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_system.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_thread.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_chrono.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_date_time.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libboost_atomic.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libpthread.so -devel/lib/robot_models/safetytest: /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 devel/lib/robot_models/safetytest: CMakeFiles/safetytest.dir/link.txt @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable devel/lib/robot_models/safetytest" $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/safetytest.dir/link.txt --verbose=$(VERBOSE) diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt b/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt index f67f60fd4a5d813a095d1c2697e14556d9c49e32..6582f4ded611d72d861993001c2423a2752fb2b7 100644 --- a/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt +++ b/cmake-build-debug/CMakeFiles/safetytest.dir/link.txt @@ -1 +1 @@ -/usr/bin/c++ -g -rdynamic CMakeFiles/safetytest.dir/src/safetytest.cpp.o -o devel/lib/robot_models/safetytest -Wl,-rpath,/opt/ros/melodic/lib /opt/ros/melodic/lib/libgazebo_ros_api_plugin.so /opt/ros/melodic/lib/libgazebo_ros_paths_plugin.so /opt/ros/melodic/lib/libmoveit_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools.so /opt/ros/melodic/lib/librviz_visual_tools_gui.so /opt/ros/melodic/lib/librviz_visual_tools_remote_control.so /opt/ros/melodic/lib/librviz_visual_tools_imarker_simple.so /opt/ros/melodic/lib/libeigen_conversions.so /opt/ros/melodic/lib/libtf_conversions.so /opt/ros/melodic/lib/libkdl_conversions.so /opt/ros/melodic/lib/libmoveit_common_planning_interface_objects.so /opt/ros/melodic/lib/libmoveit_planning_scene_interface.so /opt/ros/melodic/lib/libmoveit_move_group_interface.so /opt/ros/melodic/lib/libmoveit_py_bindings_tools.so /opt/ros/melodic/lib/libmoveit_cpp.so /opt/ros/melodic/lib/libmoveit_warehouse.so /opt/ros/melodic/lib/libwarehouse_ros.so /opt/ros/melodic/lib/libtf.so /opt/ros/melodic/lib/libmoveit_pick_place_planner.so /opt/ros/melodic/lib/libmoveit_move_group_capabilities_base.so /opt/ros/melodic/lib/libmoveit_rdf_loader.so /opt/ros/melodic/lib/libmoveit_kinematics_plugin_loader.so /opt/ros/melodic/lib/libmoveit_robot_model_loader.so /opt/ros/melodic/lib/libmoveit_constraint_sampler_manager_loader.so /opt/ros/melodic/lib/libmoveit_planning_pipeline.so /opt/ros/melodic/lib/libmoveit_trajectory_execution_manager.so /opt/ros/melodic/lib/libmoveit_plan_execution.so /opt/ros/melodic/lib/libmoveit_planning_scene_monitor.so /opt/ros/melodic/lib/libmoveit_collision_plugin_loader.so /opt/ros/melodic/lib/libdynamic_reconfigure_config_init_mutex.so /opt/ros/melodic/lib/libmoveit_ros_occupancy_map_monitor.so /opt/ros/melodic/lib/libmoveit_exceptions.so /opt/ros/melodic/lib/libmoveit_background_processing.so /opt/ros/melodic/lib/libmoveit_kinematics_base.so /opt/ros/melodic/lib/libmoveit_robot_model.so /opt/ros/melodic/lib/libmoveit_transforms.so /opt/ros/melodic/lib/libmoveit_robot_state.so /opt/ros/melodic/lib/libmoveit_robot_trajectory.so /opt/ros/melodic/lib/libmoveit_planning_interface.so /opt/ros/melodic/lib/libmoveit_collision_detection.so /opt/ros/melodic/lib/libmoveit_collision_detection_fcl.so /opt/ros/melodic/lib/libmoveit_kinematic_constraints.so /opt/ros/melodic/lib/libmoveit_planning_scene.so /opt/ros/melodic/lib/libmoveit_constraint_samplers.so /opt/ros/melodic/lib/libmoveit_planning_request_adapter.so /opt/ros/melodic/lib/libmoveit_profiler.so /opt/ros/melodic/lib/libmoveit_trajectory_processing.so /opt/ros/melodic/lib/libmoveit_distance_field.so /opt/ros/melodic/lib/libmoveit_collision_distance_field.so /opt/ros/melodic/lib/libmoveit_kinematics_metrics.so /opt/ros/melodic/lib/libmoveit_dynamics_solver.so /opt/ros/melodic/lib/libmoveit_utils.so /opt/ros/melodic/lib/libmoveit_test_utils.so -lboost_iostreams -lfcl /opt/ros/melodic/lib/libkdl_parser.so /opt/ros/melodic/lib/liburdf.so -lurdfdom_sensor -lurdfdom_model_state -lurdfdom_model -lurdfdom_world /opt/ros/melodic/lib/librosconsole_bridge.so /opt/ros/melodic/lib/libsrdfdom.so -ltinyxml /opt/ros/melodic/lib/libgeometric_shapes.so /opt/ros/melodic/lib/liboctomap.so /opt/ros/melodic/lib/liboctomath.so /opt/ros/melodic/lib/librandom_numbers.so /opt/ros/melodic/lib/libclass_loader.so -lPocoFoundation -ldl /opt/ros/melodic/lib/libroslib.so /opt/ros/melodic/lib/librospack.so -lpython2.7 -lboost_program_options -ltinyxml2 /opt/ros/melodic/lib/liborocos-kdl.so /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0 /opt/ros/melodic/lib/libtf2_ros.so /opt/ros/melodic/lib/libactionlib.so /opt/ros/melodic/lib/libmessage_filters.so /opt/ros/melodic/lib/libroscpp.so -lboost_filesystem /opt/ros/melodic/lib/librosconsole.so /opt/ros/melodic/lib/librosconsole_log4cxx.so /opt/ros/melodic/lib/librosconsole_backend_interface.so -llog4cxx -lboost_regex /opt/ros/melodic/lib/libxmlrpcpp.so /opt/ros/melodic/lib/libtf2.so /opt/ros/melodic/lib/libroscpp_serialization.so /opt/ros/melodic/lib/librostime.so /opt/ros/melodic/lib/libcpp_common.so -lboost_system -lboost_thread -lboost_chrono -lboost_date_time -lboost_atomic -lpthread /usr/lib/x86_64-linux-gnu/libconsole_bridge.so.0.4 +/usr/bin/c++ -g -rdynamic CMakeFiles/safetytest.dir/src/safetytest.cpp.o -o devel/lib/robot_models/safetytest diff --git a/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o b/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o deleted file mode 100644 index a298373b665e231f2aef8338a28d38274ddb3757..0000000000000000000000000000000000000000 Binary files a/cmake-build-debug/CMakeFiles/safetytest.dir/src/safetytest.cpp.o and /dev/null differ diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache b/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache index b95c190899c553c6e890686f2127b8b77c244a8a..a5115360009568b0e866176a5de3ea882c0b1338 100644 --- a/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache +++ b/cmake-build-debug/CMakeFiles/worldsafety.dir/CXX.includecache @@ -128,7 +128,7 @@ moveit_visual_tools/moveit_visual_tools.h moveit/trajectory_processing/iterative_time_parameterization.h - trajectory_msgs/JointTrajectoryPoint.h -- +/home/nikhil/panda_gazebo_workspace/src/zero/src/trajectory_msgs/JointTrajectoryPoint.h /opt/ros/melodic/include/actionlib/action_definition.h diff --git a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o index 0143ad09d826f6c22609377bb80e9654a19ce64a..c30872c21a909016c501db3da5d00f1c1f45ef2c 100644 Binary files a/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o and b/cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o differ diff --git a/cmake-build-debug/devel/lib/robot_models/safetytest b/cmake-build-debug/devel/lib/robot_models/safetytest deleted file mode 100755 index 5362be4b38b4635cbedf4166d0987eb40b37eb47..0000000000000000000000000000000000000000 Binary files a/cmake-build-debug/devel/lib/robot_models/safetytest and /dev/null differ diff --git a/cmake-build-debug/devel/lib/robot_models/worldsafety b/cmake-build-debug/devel/lib/robot_models/worldsafety index ca9496db546e34f15a3eab45017772dda682b84e..9bacd014d2bdf204e0dcb0720e6a3d618ab81401 100755 Binary files a/cmake-build-debug/devel/lib/robot_models/worldsafety and b/cmake-build-debug/devel/lib/robot_models/worldsafety differ diff --git a/launch/launcher.launch b/launch/launcher.launch index c8a6ff7fc24c1674aac837c2ed7a6c6dcbb2c66d..ab8ea0647e87e218724f948e5643af9d954a71b1 100644 --- a/launch/launcher.launch +++ b/launch/launcher.launch @@ -4,7 +4,7 @@ <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/> - <node pkg="testappm" type="safety_model" name="launcher" respawn="false" output="screen"/> + <node pkg="worldsafety" type="safety_model" name="launcher" respawn="false" output="screen"/> </launch> diff --git a/src/datalink/humanspace.h b/src/datalink/humanspace.h index 610a3341ad3ffdbbb00b3cb63eb366ee51d3ce9e..655c7fd4679c70214c121fb6c5a258f385b901c7 100644 --- a/src/datalink/humanspace.h +++ b/src/datalink/humanspace.h @@ -4,8 +4,8 @@ class humanspace { public: - //int humannum; - uint32 humannum; + int humannum; + // uint32 humannum; //constructor diff --git a/src/robot_models_node.cpp b/src/robot_models_node.cpp index 9b032a769426b4e25e5ad60655ac4f4df0b12495..5afa4ea8e103528533a15d061172fc3ea22fee8d 100644 --- a/src/robot_models_node.cpp +++ b/src/robot_models_node.cpp @@ -3,7 +3,7 @@ #include "datalink/graspobject.h" #include "datalink/hand.h" #include "datalink/humanspace.h" -#include "datalink/joint.h" +#include "datalink/jo.h" #include "datalink/obstacle.h" #include "datalink/robot.h" #include "datalink/robotfpe.h" @@ -22,6 +22,7 @@ #include "message_filters/subscriber.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "geometry_msgs/PointStamped.h" +#include "datalink/joint.h" #include <tf2_ros/transform_broadcaster.h> #include <tf2/LinearMath/Quaternion.h> #include <moveit/move_group_interface/move_group_interface.h> @@ -51,50 +52,50 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) joint o5f; joint o5g; obstacle o6; - robot o7; + robot robot1; robotfpe o8; world o9; worldobject o10; - o7.jo.push_back(o5a); - o7.jo.push_back(o5b); - o7.jo.push_back(o5c); - o7.jo.push_back(o5d); - o7.jo.push_back(o5e); - o7.jo.push_back(o5f); - o7.jo.push_back(o5g); - /*o7.jo1->name= "panda_link1"; - o7.jo2->name= "panda_link2"; - o7.jo3->name= "panda_link3"; - o7.jo4->name= "panda_link4"; - o7.jo5->name= "panda_link5"; - o7.jo6->name= "panda_link6"; - o7.jo7->name= "panda_link7"; - - o7.jo1->or2= {1.2,5.3,6.5,7.4}; - o7.jo2->or2= {2.5,3.6,5.8,4.9}; - o7.jo3->or2= {5.1,1.2,9.9,6.8}; - o7.jo4->or2= {6.3,3.4,8.5,9.6}; - o7.jo5->or2= {8.2,2.6,7.9,5.5}; - o7.jo6->or2= {7.3,4.4,6.7,4.0}; - o7.jo7->or2= {7.3,4.4,6.7,4.0}; - - o7.jo1->p1= {1.3, 10.1, 9.4}; - o7.jo2->p1= {14.7, 11.3, 6.5}; - o7.jo3->p1= {4.9, 13.8, 7.7}; - o7.jo4->p1= {2.4, 12.5, 1.6}; - o7.jo5->p1= {3.1, 16.2, 5.3}; - o7.jo6->p1= {11.5, 15.4, 3.4}; - o7.jo7->p1= {5.1, 18.3, 4.2};*/ + robot1.jo.push_back(o5a); + robot1.jo.push_back(o5b); + robot1.jo.push_back(o5c); + robot1.jo.push_back(o5d); + robot1.jo.push_back(o5e); + robot1.jo.push_back(o5f); + robot1.jo.push_back(o5g); + /*robot1.jo1->name= "panda_link1"; + robot1.jo2->name= "panda_link2"; + robot1.jo3->name= "panda_link3"; + robot1.jo4->name= "panda_link4"; + robot1.jo5->name= "panda_link5"; + robot1.jo6->name= "panda_link6"; + robot1.jrobot1->name= "panda_link7"; + + robot1.jo1->or2= {1.2,5.3,6.5,7.4}; + robot1.jo2->or2= {2.5,3.6,5.8,4.9}; + robot1.jo3->or2= {5.1,1.2,9.9,6.8}; + robot1.jo4->or2= {6.3,3.4,8.5,9.6}; + robot1.jo5->or2= {8.2,2.6,7.9,5.5}; + robot1.jo6->or2= {7.3,4.4,6.7,4.0}; + robot1.jrobot1->or2= {7.3,4.4,6.7,4.0}; + + robot1.jo1->p1= {1.3, 10.1, 9.4}; + robot1.jo2->p1= {14.7, 11.3, 6.5}; + robot1.jo3->p1= {4.9, 13.8, 7.7}; + robot1.jo4->p1= {2.4, 12.5, 1.6}; + robot1.jo5->p1= {3.1, 16.2, 5.3}; + robot1.jo6->p1= {11.5, 15.4, 3.4}; + robot1.jrobot1->p1= {5.1, 18.3, 4.2};*/ int yx=0; - o7.jo[++yx].name= "panda_link1"; - o7.jo[++yx].name= "panda_link2"; - o7.jo[++yx].name= "panda_link3"; - o7.jo[++yx].name= "panda_link4"; - o7.jo[++yx].name= "panda_link5"; - o7.jo[++yx].name= "panda_link6"; - o7.jo[++yx].name= "panda_link7"; + robot1.jo[++yx].name= "panda_link1"; + robot1.jo[++yx].name= "panda_link2"; + robot1.jo[++yx].name= "panda_link3"; + robot1.jo[++yx].name= "panda_link4"; + robot1.jo[++yx].name= "panda_link5"; + robot1.jo[++yx].name= "panda_link6"; + robot1.jo[++yx].name= "panda_link7"; o9.c->robname="Robot FPE"; @@ -158,13 +159,13 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) uint32::data updateDetectedHumans; geometry_msgs::uint32 x; try { - transformStamped1 = tfBuffer.lookupTransform("world", o7.jo[1].name, ros::Time(0)); - transformStamped2 = tfBuffer.lookupTransform("world", o7.jo[2].name, ros::Time(0)); - transformStamped3 = tfBuffer.lookupTransform("world", o7.jo[3].name, ros::Time(0)); - transformStamped4 = tfBuffer.lookupTransform("world", o7.jo[4].name, ros::Time(0)); - transformStamped5 = tfBuffer.lookupTransform("world", o7.jo[5].name, ros::Time(0)); - transformStamped6 = tfBuffer.lookupTransform("world", o7.jo[6].name, ros::Time(0)); - transformStamped7 = tfBuffer.lookupTransform("world", o7.jo[7].name, ros::Time(0)); + transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0)); + transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0)); + transformStamped3 = tfBuffer.lookupTransform("world", robot1.jo[3].name, ros::Time(0)); + transformStamped4 = tfBuffer.lookupTransform("world", robot1.jo[4].name, ros::Time(0)); + transformStamped5 = tfBuffer.lookupTransform("world", robot1.jo[5].name, ros::Time(0)); + transformStamped6 = tfBuffer.lookupTransform("world", robot1.jo[6].name, ros::Time(0)); + transformStamped7 = tfBuffer.lookupTransform("world", robot1.jo[7].name, ros::Time(0)); updateDetectedHumans=o9.a->humannum; } catch (tf2::TransformException &ex) { ROS_WARN("%s", ex.what()); @@ -172,133 +173,127 @@ void updateDectectedHumans(const std_msgs::uint32::ConstPtr& msg) continue; } - o7.jo[1].p1[0]=0.0; - o7.jo[1].p1[1]=0.0; - o7.jo[1].p1[2]=0.0; - o7.jo[2].p1[0]=0.0; - o7.jo[2].p1[1]=0.0; - o7.jo[2].p1[2]=0.0; - o7.jo[3].p1[0]=0.0; - o7.jo[3].p1[1]=0.0; - o7.jo[3].p1[2]=0.0; - o7.jo[4].p1[0]=0.0; - o7.jo[4].p1[1]=0.0; - o7.jo[4].p1[2]=0.0; - o7.jo[5].p1[0]=0.0; - o7.jo[5].p1[1]=0.0; - o7.jo[5].p1[2]=0.0; - o7.jo[6].p1[0]=0.0; - o7.jo[6].p1[1]=0.0; - o7.jo[6].p1[2]=0.0; - o7.jo[7].p1[0]=0.0; - o7.jo[7].p1[1]=0.0; - o7.jo[7].p1[2]=0.0; - - o7.jo[1].p1[0]=transformStamped1.transform.translation.x; - o7.jo[1].p1[1]=transformStamped1.transform.translation.y; - o7.jo[1].p1[2]=transformStamped1.transform.translation.z; - ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< o7.jo[1].p1[0] << ", y = "<< o7.jo[1].p1[1]<<", z = "<<o7.jo[1].p1[2]); - - o7.jo[2].p1[0]=transformStamped2.transform.translation.x; - o7.jo[2].p1[1]=transformStamped2.transform.translation.y; - o7.jo[2].p1[2]=transformStamped2.transform.translation.z; - ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< o7.jo[2].p1[0] << ", y = "<< o7.jo[2].p1[1]<<", z = "<<o7.jo[2].p1[2]); - - o7.jo[3].p1[0]=transformStamped3.transform.translation.x; - o7.jo[3].p1[1]=transformStamped3.transform.translation.y; - o7.jo[3].p1[2]=transformStamped3.transform.translation.z; - ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< o7.jo[3].p1[0] << ", y = "<< o7.jo[3].p1[1]<<", z = "<<o7.jo[3].p1[2]); - - o7.jo[4].p1[0]=transformStamped4.transform.translation.x; - o7.jo[4].p1[1]=transformStamped4.transform.translation.y; - o7.jo[4].p1[2]=transformStamped4.transform.translation.z; - ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< o7.jo[4].p1[0] << ", y = "<< o7.jo[4].p1[1]<<", z = "<<o7.jo[4].p1[2]); - - o7.jo[5].p1[0]=transformStamped5.transform.translation.x; - o7.jo[5].p1[1]=transformStamped5.transform.translation.y; - o7.jo[5].p1[2]=transformStamped5.transform.translation.z; - ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< o7.jo[5].p1[0] << ", y = "<< o7.jo[5].p1[1]<<", z = "<<o7.jo[5].p1[2]); - - o7.jo[6].p1[0]=transformStamped6.transform.translation.x; - o7.jo[6].p1[1]=transformStamped6.transform.translation.y; - o7.jo[6].p1[2]=transformStamped6.transform.translation.z; - ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< o7.jo[6].p1[0] << ", y = "<< o7.jo[6].p1[1]<<", z = "<<o7.jo[6].p1[2]); - - o7.jo[7].p1[0]=transformStamped7.transform.translation.x; - o7.jo[7].p1[1]=transformStamped7.transform.translation.y; - o7.jo[7].p1[2]=transformStamped7.transform.translation.z; - ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< o7.jo[7].p1[0] << ", y = "<< o7.jo[7].p1[1]<<", z = "<<o7.jo[7].p1[2]); - - o7.jo[1].or2.w=0.0; - o7.jo[1].or2.x=0.0; - o7.jo[1].or2.y=0.0; - o7.jo[1].or2.z=0.0; - o7.jo[2].or2.w=0.0; - o7.jo[2].or2.x=0.0; - o7.jo[2].or2.y=0.0; - o7.jo[2].or2.z=0.0; - o7.jo[3].or2.w=0.0; - o7.jo[3].or2.x=0.0; - o7.jo[3].or2.y=0.0; - o7.jo[3].or2.z=0.0; - o7.jo[4].or2.w=0.0; - o7.jo[4].or2.x=0.0; - o7.jo[4].or2.y=0.0; - o7.jo[4].or2.z=0.0; - o7.jo[5].or2.w=0.0; - o7.jo[5].or2.x=0.0; - o7.jo[5].or2.y=0.0; - o7.jo[5].or2.z=0.0; - o7.jo[6].or2.w=0.0; - o7.jo[6].or2.x=0.0; - o7.jo[6].or2.y=0.0; - o7.jo[6].or2.z=0.0; - o7.jo[7].or2.w=0.0; - o7.jo[7].or2.x=0.0; - o7.jo[7].or2.y=0.0; - o7.jo[7].or2.z=0.0; - - o7.jo[1].or2.w=transformStamped1.transform.rotation.w; - o7.jo[1].or2.x=transformStamped1.transform.rotation.x; - o7.jo[1].or2.y=transformStamped1.transform.rotation.y; - o7.jo[1].or2.z=transformStamped1.transform.rotation.z; - ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< o7.jo[1].or2.w<<", x = "<<o7.jo[1].or2.x<<", y = "<<o7.jo[1].or2.y<<", z = "<<o7.jo[1].or2.z); - - o7.jo[2].or2.w=transformStamped2.transform.rotation.w; - o7.jo[2].or2.x=transformStamped2.transform.rotation.x; - o7.jo[2].or2.y=transformStamped2.transform.rotation.y; - o7.jo[2].or2.z=transformStamped2.transform.rotation.z; - ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< o7.jo[2].or2.w<<", x = "<< o7.jo[2].or2.x<<", y = "<< o7.jo[2].or2.y<<", z = "<< o7.jo[2].or2.z); - - o7.jo[3].or2.w=transformStamped3.transform.rotation.w; - o7.jo[3].or2.x=transformStamped3.transform.rotation.x; - o7.jo[3].or2.y=transformStamped3.transform.rotation.y; - o7.jo[3].or2.z=transformStamped3.transform.rotation.z; - ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< o7.jo[3].or2.w<<", x = "<<o7.jo[3].or2.x<<", y = "<<o7.jo[3].or2.y<<", z = "<<o7.jo[3].or2.z); - - o7.jo[4].or2.w=transformStamped4.transform.rotation.w; - o7.jo[4].or2.x=transformStamped4.transform.rotation.x; - o7.jo[4].or2.y=transformStamped4.transform.rotation.y; - o7.jo[4].or2.z=transformStamped4.transform.rotation.z; - ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<o7.jo[4].or2.w<<", x = "<<o7.jo[4].or2.x<<", y = "<<o7.jo[4].or2.y<<", z = "<<o7.jo[4].or2.z); - - o7.jo[5].or2.w=transformStamped5.transform.rotation.w; - o7.jo[5].or2.x=transformStamped5.transform.rotation.x; - o7.jo[5].or2.y=transformStamped5.transform.rotation.y; - o7.jo[5].or2.z=transformStamped5.transform.rotation.z; - ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< o7.jo[5].or2.w<<", x = "<<o7.jo[5].or2.x<<", y = "<<o7.jo[5].or2.y<<", z = "<<o7.jo[5].or2.z); - - o7.jo[6].or2.w=transformStamped6.transform.rotation.w; - o7.jo[6].or2.x=transformStamped6.transform.rotation.x; - o7.jo[6].or2.y=transformStamped6.transform.rotation.y; - o7.jo[6].or2.z=transformStamped6.transform.rotation.z; - ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< o7.jo[6].or2.w<<", x = "<<o7.jo[6].or2.x<<", y = "<<o7.jo[6].or2.y<<", z = "<<o7.jo[6].or2.z); - - o7.jo[7].or2.w=transformStamped7.transform.rotation.w; - o7.jo[7].or2.x=transformStamped7.transform.rotation.x; - o7.jo[7].or2.y=transformStamped7.transform.rotation.y; - o7.jo[7].or2.z=transformStamped7.transform.rotation.z; - ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<o7.jo[7].or2.w<<", x = "<<o7.jo[7].or2.x<<", y = "<<o7.jo[7].or2.y<<", z = "<<o7.jo[7].or2.z); + robot1.jo[3].p1[0]=0.0; + robot1.jo[3].p1[1]=0.0; + robot1.jo[3].p1[2]=0.0; + robot1.jo[4].p1[0]=0.0; + robot1.jo[4].p1[1]=0.0; + robot1.jo[4].p1[2]=0.0; + robot1.jo[5].p1[0]=0.0; + robot1.jo[5].p1[1]=0.0; + robot1.jo[5].p1[2]=0.0; + robot1.jo[6].p1[0]=0.0; + robot1.jo[6].p1[1]=0.0; + robot1.jo[6].p1[2]=0.0; + robot1.jo[7].p1[0]=0.0; + robot1.jo[7].p1[1]=0.0; + robot1.jo[7].p1[2]=0.0; + + robot1.jo[1].p1[0]=transformStamped1.transform.translation.x; + robot1.jo[1].p1[1]=transformStamped1.transform.translation.y; + robot1.jo[1].p1[2]=transformStamped1.transform.translation.z; + ROS_INFO_STREAM("panda_link1 Position is " <<" x = "<< robot1.jo[1].p1[0] << ", y = "<< robot1.jo[1].p1[1]<<", z = "<<robot1.jo[1].p1[2]); + + robot1.jo[2].p1[0]=transformStamped2.transform.translation.x; + robot1.jo[2].p1[1]=transformStamped2.transform.translation.y; + robot1.jo[2].p1[2]=transformStamped2.transform.translation.z; + ROS_INFO_STREAM("panda_link2 Position is " <<" x = "<< robot1.jo[2].p1[0] << ", y = "<< robot1.jo[2].p1[1]<<", z = "<<robot1.jo[2].p1[2]); + + robot1.jo[3].p1[0]=transformStamped3.transform.translation.x; + robot1.jo[3].p1[1]=transformStamped3.transform.translation.y; + robot1.jo[3].p1[2]=transformStamped3.transform.translation.z; + ROS_INFO_STREAM("panda_link3 Position is " <<" x = "<< robot1.jo[3].p1[0] << ", y = "<< robot1.jo[3].p1[1]<<", z = "<<robot1.jo[3].p1[2]); + + robot1.jo[4].p1[0]=transformStamped4.transform.translation.x; + robot1.jo[4].p1[1]=transformStamped4.transform.translation.y; + robot1.jo[4].p1[2]=transformStamped4.transform.translation.z; + ROS_INFO_STREAM("panda_link4 Position is " <<" x = "<< robot1.jo[4].p1[0] << ", y = "<< robot1.jo[4].p1[1]<<", z = "<<robot1.jo[4].p1[2]); + + robot1.jo[5].p1[0]=transformStamped5.transform.translation.x; + robot1.jo[5].p1[1]=transformStamped5.transform.translation.y; + robot1.jo[5].p1[2]=transformStamped5.transform.translation.z; + ROS_INFO_STREAM("panda_link5 Position is " <<" x = "<< robot1.jo[5].p1[0] << ", y = "<< robot1.jo[5].p1[1]<<", z = "<<robot1.jo[5].p1[2]); + + robot1.jo[6].p1[0]=transformStamped6.transform.translation.x; + robot1.jo[6].p1[1]=transformStamped6.transform.translation.y; + robot1.jo[6].p1[2]=transformStamped6.transform.translation.z; + ROS_INFO_STREAM("panda_link6 Position is " <<" x = "<< robot1.jo[6].p1[0] << ", y = "<< robot1.jo[6].p1[1]<<", z = "<<robot1.jo[6].p1[2]); + + robot1.jo[7].p1[0]=transformStamped7.transform.translation.x; + robot1.jo[7].p1[1]=transformStamped7.transform.translation.y; + robot1.jo[7].p1[2]=transformStamped7.transform.translation.z; + ROS_INFO_STREAM("panda_link7 Position is " <<" x = "<< robot1.jo[7].p1[0] << ", y = "<< robot1.jo[7].p1[1]<<", z = "<<robot1.jo[7].p1[2]); + + robot1.jo[1].or2.w=0.0; + robot1.jo[1].or2.x=0.0; + robot1.jo[1].or2.y=0.0; + robot1.jo[1].or2.z=0.0; + robot1.jo[2].or2.w=0.0; + robot1.jo[2].or2.x=0.0; + robot1.jo[2].or2.y=0.0; + robot1.jo[2].or2.z=0.0; + robot1.jo[3].or2.w=0.0; + robot1.jo[3].or2.x=0.0; + robot1.jo[3].or2.y=0.0; + robot1.jo[3].or2.z=0.0; + robot1.jo[4].or2.w=0.0; + robot1.jo[4].or2.x=0.0; + robot1.jo[4].or2.y=0.0; + robot1.jo[4].or2.z=0.0; + robot1.jo[5].or2.w=0.0; + robot1.jo[5].or2.x=0.0; + robot1.jo[5].or2.y=0.0; + robot1.jo[5].or2.z=0.0; + robot1.jo[6].or2.w=0.0; + robot1.jo[6].or2.x=0.0; + robot1.jo[6].or2.y=0.0; + robot1.jo[6].or2.z=0.0; + robot1.jo[7].or2.w=0.0; + robot1.jo[7].or2.x=0.0; + robot1.jo[7].or2.y=0.0; + robot1.jo[7].or2.z=0.0; + + robot1.jo[1].or2.w=transformStamped1.transform.rotation.w; + robot1.jo[1].or2.x=transformStamped1.transform.rotation.x; + robot1.jo[1].or2.y=transformStamped1.transform.rotation.y; + robot1.jo[1].or2.z=transformStamped1.transform.rotation.z; + ROS_INFO_STREAM("panda_link1 Orientation is " <<" w = "<< robot1.jo[1].or2.w<<", x = "<<robot1.jo[1].or2.x<<", y = "<<robot1.jo[1].or2.y<<", z = "<<robot1.jo[1].or2.z); + + robot1.jo[2].or2.w=transformStamped2.transform.rotation.w; + robot1.jo[2].or2.x=transformStamped2.transform.rotation.x; + robot1.jo[2].or2.y=transformStamped2.transform.rotation.y; + robot1.jo[2].or2.z=transformStamped2.transform.rotation.z; + ROS_INFO_STREAM("panda_link2 Orientation is " <<" w = "<< robot1.jo[2].or2.w<<", x = "<< robot1.jo[2].or2.x<<", y = "<< robot1.jo[2].or2.y<<", z = "<< robot1.jo[2].or2.z); + + robot1.jo[3].or2.w=transformStamped3.transform.rotation.w; + robot1.jo[3].or2.x=transformStamped3.transform.rotation.x; + robot1.jo[3].or2.y=transformStamped3.transform.rotation.y; + robot1.jo[3].or2.z=transformStamped3.transform.rotation.z; + ROS_INFO_STREAM("panda_link3 Orientation is " <<" w = "<< robot1.jo[3].or2.w<<", x = "<<robot1.jo[3].or2.x<<", y = "<<robot1.jo[3].or2.y<<", z = "<<robot1.jo[3].or2.z); + + robot1.jo[4].or2.w=transformStamped4.transform.rotation.w; + robot1.jo[4].or2.x=transformStamped4.transform.rotation.x; + robot1.jo[4].or2.y=transformStamped4.transform.rotation.y; + robot1.jo[4].or2.z=transformStamped4.transform.rotation.z; + ROS_INFO_STREAM("panda_link4 Orientation is " <<" w = "<<robot1.jo[4].or2.w<<", x = "<<robot1.jo[4].or2.x<<", y = "<<robot1.jo[4].or2.y<<", z = "<<robot1.jo[4].or2.z); + + robot1.jo[5].or2.w=transformStamped5.transform.rotation.w; + robot1.jo[5].or2.x=transformStamped5.transform.rotation.x; + robot1.jo[5].or2.y=transformStamped5.transform.rotation.y; + robot1.jo[5].or2.z=transformStamped5.transform.rotation.z; + ROS_INFO_STREAM("panda_link5 Orientation is " <<" w = "<< robot1.jo[5].or2.w<<", x = "<<robot1.jo[5].or2.x<<", y = "<<robot1.jo[5].or2.y<<", z = "<<robot1.jo[5].or2.z); + + robot1.jo[6].or2.w=transformStamped6.transform.rotation.w; + robot1.jo[6].or2.x=transformStamped6.transform.rotation.x; + robot1.jo[6].or2.y=transformStamped6.transform.rotation.y; + robot1.jo[6].or2.z=transformStamped6.transform.rotation.z; + ROS_INFO_STREAM("panda_link6 Orientation is " <<" w = "<< robot1.jo[6].or2.w<<", x = "<<robot1.jo[6].or2.x<<", y = "<<robot1.jo[6].or2.y<<", z = "<<robot1.jo[6].or2.z); + + robot1.jo[7].or2.w=transformStamped7.transform.rotation.w; + robot1.jo[7].or2.x=transformStamped7.transform.rotation.x; + robot1.jo[7].or2.y=transformStamped7.transform.rotation.y; + robot1.jo[7].or2.z=transformStamped7.transform.rotation.z; + ROS_INFO_STREAM("panda_link7 Orientation is " <<" w = "<<robot1.jo[7].or2.w<<", x = "<<robot1.jo[7].or2.x<<", y = "<<robot1.jo[7].or2.y<<", z = "<<robot1.jo[7].or2.z); } rate.sleep(); } diff --git a/src/worldsafety.cpp b/src/worldsafety.cpp index 6323304439c51ce7c6b7fd141af17b46b55763fe..468c8ec1546f03ab3638195f252c8c2391aa33f0 100644 --- a/src/worldsafety.cpp +++ b/src/worldsafety.cpp @@ -29,7 +29,7 @@ #include <moveit_msgs/CollisionObject.h> #include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit/trajectory_processing/iterative_time_parameterization.h> -#include <trajectory_msgs/JointTrajectoryPoint.h> +#include "trajectory_msgs/JointTrajectoryPoint.h" int main(int argc, char** argv) { @@ -51,34 +51,27 @@ int main(int argc, char** argv) joint o5f; joint o5g; obstacle o6; - robot o7; + robot robot1; robotfpe o8; world o9; worldobject o10; - o7.jo.push_back(o5a); - o7.jo.push_back(o5b); - o7.jo.push_back(o5c); - o7.jo.push_back(o5d); - o7.jo.push_back(o5e); - o7.jo.push_back(o5f); - o7.jo.push_back(o5g); - - /* // test if the robot contains 7 joints static test - if (o7.jo.size() == 5) { - ROS_INFO("SIZE IS 5"); - } else { - ROS_INFO_STREAM("SIZE IS NOT 5 but was " << o7.jo.size()); - }*/ + robot1.jo.push_back(o5a); + robot1.jo.push_back(o5b); + robot1.jo.push_back(o5c); + robot1.jo.push_back(o5d); + robot1.jo.push_back(o5e); + robot1.jo.push_back(o5f); + robot1.jo.push_back(o5g); int yx=0; - o7.jo[++yx].name= "panda_link1"; - o7.jo[++yx].name= "panda_link2"; - o7.jo[++yx].name= "panda_link3"; - o7.jo[++yx].name= "panda_link4"; - o7.jo[++yx].name= "panda_link5"; - o7.jo[++yx].name= "panda_link6"; - o7.jo[++yx].name= "panda_link7"; + robot1.jo[++yx].name= "panda_link1"; + robot1.jo[++yx].name= "panda_link2"; + robot1.jo[++yx].name= "panda_link3"; + robot1.jo[++yx].name= "panda_link4"; + robot1.jo[++yx].name= "panda_link5"; + robot1.jo[++yx].name= "panda_link6"; + robot1.jo[++yx].name= "panda_link7"; o9.c->robname="Robot FPE"; o9.c->of=true; @@ -105,34 +98,12 @@ int main(int argc, char** argv) o9.b2->z = 1.6; o9.b2->w = 1.9; - // Check Robot Motion if moved to a specific point or not - // setup the planning environment - - /* static const std::string PLANNING_GROUP = "panda_arm"; - moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - - ros::Duration(3.0).sleep(); - - move_group.setStartStateToCurrentState(); - - geometry_msgs::Pose another_pose; - another_pose.orientation.w = 1.0; - another_pose.position.x = 0.4; - another_pose.position.y = -0.4; - another_pose.position.z = 0.9; - move_group.setPoseTarget(another_pose); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO("Visualizing plan %s", success ? "" : "FAILED"); - - // Move the (simulated) robot in gazebo - move_group.move();*/ - if (o7.jo.size() == 7) - ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS "); + // Static Tests - World Model Safety Test Cases to check Segmentation error + // test the properties of the model that do not change over time, such as the structure of you model + if (robot1.jo.size() == 7) + { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. ROBOT FPE HAS 7 JOINTS ");} else - ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << o7.jo.size()); + { ROS_ERROR_STREAM(" TEST CASE FAILED. ROBOT FPE HAS WRONG NUMBER OF JOINTS = " << robot1.jo.size());} if (o8.h == nullptr) ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION"); @@ -148,13 +119,12 @@ int main(int argc, char** argv) if (o9.b2 == nullptr) ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION"); - + // End of static tests + // Start Robot Motion static const std::string PLANNING_GROUP = "panda_arm"; moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - ros::Duration(3.0).sleep(); - move_group.setStartStateToCurrentState(); geometry_msgs::Pose another_pose; @@ -167,55 +137,42 @@ int main(int argc, char** argv) moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO("Visualizing plan %s", success ? "" : "FAILED"); - // Move the (simulated) robot in gazebo move_group.move(); + //ros::shutdown(); + // End Robot Motion + // Start input values from Gazebo to program tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); - ros::Rate rate(10); - + //robot1.jo[7].p1[2]=0.0; int i=0; int ho=0; - while (node_handle.ok()) { - { + while (node_handle.ok()) + { geometry_msgs::TransformStamped transformStamped[7]; - try { - transformStamped[++i] = tfBuffer.lookupTransform("world", o7.jo[++ho].name, ros::Time(0)); + transformStamped[++i] = tfBuffer.lookupTransform("world", robot1.jo[++ho].name, ros::Time(0)); } catch (tf2::TransformException &ex) { ROS_WARN("%s", ex.what()); ros::Duration(0.1).sleep(); continue; } - o7.jo[7].p1[2]=transformStamped[7].transform.translation.z; - -// World Model Safety Test Cases to check Segmentation errors - ros::Rate rate(10); -// test the properties of the model that do not change over time, such as the structure of you model - // Static Tests - - - while (node_handle.ok()) { - { - // update the model data like in the application model node - - // test the dynamic parts here, i.e., if the model data is consistent with the position of the robot - //Checking if robot moved to position or not - if (o7.jo[7].p1[2] == 0.9) - ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); - else - ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); - ros::shutdown(); - - } - - } + robot1.jo[i].p1[2] = transformStamped[ho].transform.translation.z; + }//End of taking input in program + // Begin test case to check if robot moved to its position + while (node_handle.ok()) + { + { + // update the model data like in the application model node, test the dynamic parts here, i.e., if the model data is consistent with the position of the robot + //Checking if robot moved to position or not + if (robot1.jo[7].p1[2] == 0.9) + ROS_INFO_STREAM(" THE ROBOT HAS MOVED TO POSITION AS INTENDED "); + else + ROS_ERROR_STREAM("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED"); + } rate.sleep(); - } - - - return 0; } + return 0; }