Skip to content
Snippets Groups Projects
Commit fa38e8e1 authored by Nikhil Ambardar's avatar Nikhil Ambardar
Browse files

needreview2models

parent 30e901da
Branches
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Showing
with 407 additions and 244 deletions
CMakeLists.txt
+ 26
− 20
View file @ fa38e8e1
......@@ -13,6 +13,10 @@ find_package(catkin REQUIRED COMPONENTS
moveit_core
moveit_visual_tools
moveit_ros_planning_interface
roscpp
rospy
std_msgs
genmsg
)
## System dependencies are found with CMake's conventions
......@@ -61,7 +65,8 @@ find_package(catkin REQUIRED COMPONENTS
# Service1.srv
# Service2.srv
# )
add_message_files(DIRECTORY msg FILES Num.msg)
add_service_files(DIRECTORY srv FILES AddTwoInts.srv)
## Generate actions in the 'action' folder
# add_action_files(
# FILES
......@@ -74,7 +79,7 @@ find_package(catkin REQUIRED COMPONENTS
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
generate_messages(DEPENDENCIES std_msgs)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
......@@ -153,7 +158,6 @@ include_directories(
add_executable(robot_models_node
src/robot_models_node.cpp
src/datalink/abstractobject.h
src/datalink/graspobject.h
......@@ -168,21 +172,20 @@ add_executable(robot_models_node
src/datalink/quaternion.h
)
add_executable(safetytest
src/safetytest.cpp
src/datalink/abstractobject.h
src/datalink/graspobject.h
src/datalink/hand.h
src/datalink/humanspace.h
src/datalink/joint.h
src/datalink/obstacle.h
src/datalink/robot.h
src/datalink/robotfpe.h
src/datalink/world.h
src/datalink/worldobject.h
src/datalink/quaternion.h
)
##add_executable(safetytest
## src/safetytest.cpp
## src/datalink/abstractobject.h
## src/datalink/graspobject.h
## src/datalink/hand.h
## src/datalink/humanspace.h
## src/datalink/joint.h
## src/datalink/obstacle.h
## src/datalink/robot.h
## src/datalink/robotfpe.h
## src/datalink/world.h
## src/datalink/worldobject.h
## src/datalink/quaternion.h
## )
add_executable(worldsafety
src/worldsafety.cpp
src/datalink/abstractobject.h
......@@ -197,7 +200,9 @@ add_executable(worldsafety
src/datalink/worldobject.h
src/datalink/quaternion.h
)
add_executable(talker
src/talker.cpp
)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
......@@ -210,8 +215,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(talker ${catkin_LIBRARIES} )
##target_link_libraries(safetytest ${catkin_LIBRARIES} )
#add_dependencies(robot_models_node ${catkin_LIBRARIES})
......
cmake-build-debug/CMakeFiles/clion-log.txt
+ 10
− 3
View file @ fa38e8e1
......@@ -16,6 +16,13 @@
-- catkin 0.7.28
-- BUILD_SHARED_LIBS is on
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
-- Configuring done
-- Generating done
-- Build files have been written to: /home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug
CMake Error at /opt/ros/melodic/share/genmsg/cmake/genmsg-extras.cmake:94 (message):
add_message_files() directory not found:
/home/nikhil/panda_gazebo_workspace/src/zero/msg
Call Stack (most recent call first):
CMakeLists.txt:68 (add_message_files)
-- Configuring incomplete, errors occurred!
See also "/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/CMakeOutput.log".
See also "/home/nikhil/panda_gazebo_workspace/src/zero/cmake-build-debug/CMakeFiles/CMakeError.log".
cmake-build-debug/CMakeFiles/worldsafety.dir/build.make
+ 1
− 1
View file @ fa38e8e1
......@@ -160,6 +160,7 @@ devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/liborocos-kdl.so.1.4.0
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2_ros.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libactionlib.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libmessage_filters.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libroscpp.so
devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/libboost_filesystem.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librosconsole.so
......@@ -168,7 +169,6 @@ devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librosconsole_backend_i
devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/liblog4cxx.so
devel/lib/robot_models/worldsafety: /usr/lib/x86_64-linux-gnu/libboost_regex.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libxmlrpcpp.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libtf2.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libroscpp_serialization.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/librostime.so
devel/lib/robot_models/worldsafety: /opt/ros/melodic/lib/libcpp_common.so
......
cmake-build-debug/CMakeFiles/worldsafety.dir/link.txt
+ 1
− 1
View file @ fa38e8e1
/usr/bin/c++ -g -rdynamic CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o -o devel/lib/robot_models/worldsafety -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/worldsafety.dir/src/worldsafety.cpp.o -o devel/lib/robot_models/worldsafety -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/libtf2.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/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
cmake-build-debug/CMakeFiles/worldsafety.dir/src/worldsafety.cpp.o
+ 0
− 0
View file @ fa38e8e1
No preview for this file type
cmake-build-debug/catkin_generated/stamps/robot_models/package.xml.stamp
+ 43
− 59
View file @ fa38e8e1
<?xml version="1.0"?>
<package format="2">
<name>robot_models</name>
<version>0.1.0</version>
<description>The robot_models package</description>
<?xml-model
href="http://download.ros.org/schema/package_format3.xsd"
schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>catkin</name>
<version>0.7.28</version>
<description>Low-level build system macros and infrastructure for ROS.</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url type="website">http://wiki.ros.org/catkin</url>
<url type="bugtracker">https://github.com/ros/catkin/issues</url>
<url type="repository">https://github.com/ros/catkin</url>
<author>Troy Straszheim</author>
<author>Morten Kjaergaard</author>
<author>Brian Gerkey</author>
<author>Dirk Thomas</author>
<depend condition="$ROS_PYTHON_VERSION == 2">python-argparse</depend>
<depend condition="$ROS_PYTHON_VERSION == 2" version_gt="0.4.3">python-catkin-pkg</depend>
<depend condition="$ROS_PYTHON_VERSION == 3" version_gt="0.4.3">python3-catkin-pkg</depend>
<depend condition="$ROS_PYTHON_VERSION == 2">python-empy</depend>
<depend condition="$ROS_PYTHON_VERSION == 3">python3-empy</depend>
<buildtool_depend>cmake</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
<buildtool_export_depend>cmake</buildtool_export_depend>
<buildtool_export_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_export_depend>
<build_export_depend>google-mock</build_export_depend>
<build_export_depend>gtest</build_export_depend>
<build_export_depend condition="$ROS_PYTHON_VERSION == 2">python-nose</build_export_depend>
<build_export_depend condition="$ROS_PYTHON_VERSION == 3">python3-nose</build_export_depend>
<test_depend condition="$ROS_PYTHON_VERSION == 2">python-mock</test_depend>
<test_depend condition="$ROS_PYTHON_VERSION == 3">python3-mock</test_depend>
<test_depend condition="$ROS_PYTHON_VERSION == 2">python-nose</test_depend>
<test_depend condition="$ROS_PYTHON_VERSION == 3">python3-nose</test_depend>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="nikhil.ambardar@tu-dresden.de">Nikhil Ambardar</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>MIT</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/robot_models</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<author email="nikhil.ambardar@tu-dresden.de">Nikhil Ambardar</author>
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<depend>gazebo_ros</depend>
<depend>moveit_core</depend>
<depend>moveit_visual_tools</depend>
<depend>moveit_ros_planning_interface</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<rosdoc config="rosdoc.yaml"/>
<architecture_independent/>
</export>
</package>
cmake-build-debug/devel/lib/robot_models/worldsafety
+ 0
− 0
View file @ fa38e8e1
No preview for this file type
launch/launcher.launch
+ 1
− 1
View file @ fa38e8e1
......@@ -4,7 +4,7 @@
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/>
<node pkg="worldsafety" type="safety_model" name="launcher" respawn="false" output="screen"/>
<node pkg="robots_models_node" type="safety_model" name="launcher" respawn="false" output="screen"/>
</launch>
src/datalink/world.h
+ 10
− 9
View file @ fa38e8e1
#include "humanspace.h"
#include "abstractobject.h"
#include "robot.h"
#ifndef ROBOT_MODELS_W_H
#define ROBOT_MODELS_W_H
class world
{
public: humanspace *a=new humanspace;
public:
humanspace *humans;
vector<abstractobject*> objects;
// we cannot construct the robot here, because we do not know if
// we want to construct a regular robot or a robotfpe
robot *c;
/*humanspace *a=new humanspace;
abstractobject *b1=new abstractobject;
abstractobject *b2=new abstractobject;
robot *c=new robot;
robot *c=new robot;*/
};
#endif //ROBOT_MODELS_TEST_H
src/datalink/worldobject.h
+ 5
− 4
View file @ fa38e8e1
......@@ -6,11 +6,12 @@ using namespace std;
class worldobject
{
public:
vector<float> p1;// saw no purpose of using h3 here in WO ,its used seperately in hand class
vector<float> p1;
std::string name;
double x,y,z,w;
//no need of constructor here
// constructor
worldobject() : x{}, y{}, z{}, w{} {
p1.resize(3);
}
};
#endif //ROBOT_MODELS_TEST_H
\ No newline at end of file
src/robot_models_node.cpp
+ 37
− 28
View file @ fa38e8e1
......@@ -27,8 +27,12 @@
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <std_msgs/UInt32.h>
void updateDectectedHumans(const std_msgs::UInt32::ConstPtr &msg) {
ROS_INFO("I heard: [%i]", msg->data.c_str());
#include "std_msgs/String.h"
#include <sstream>
void pub(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("I heard: [%s]", msg->data.c_str());
}
int main(int argc, char **argv) {
......@@ -80,40 +84,47 @@ int main(int argc, char **argv) {
o8.h->grippos = {12.1, 14.3, 18.2};
o9.a->humannum = 1;
o9.b1->name = "Cube";
o9.b1->size = {5.2, 7.5, 9.1};
auto *obstacle1 = new obstacle{};
obstacle1->name = "Cube";
obstacle1->size = {5.2, 7.5, 9.1};
obstacle1->p1 = {13.2, 14.1, 17.5};
obstacle1->x = 2.3;
obstacle1->y = 1.2;
obstacle1->z = 0.2;
obstacle1->w = 0.1;
/*
o9.objects->name = "Cube";
o9.objects->size = {5.2, 7.5, 9.1};
o9.b1->p1 = {13.2, 14.1, 17.5};
o9.b1->x = 2.3;
o9.b1->y = 1.2;
o9.b1->z = 0.2;
o9.b1->w = 0.1;
o9.b2->name = "Ball";
*/
auto *graspobject1 = new graspobject;
graspobject1->name = "Ball";
graspobject1->size = {1.3, 2.1, 3.2};
graspobject1->p1 = {12.5, 16.4, 17.3};
graspobject1->x = 1.1;
graspobject1->y = 1.4;
graspobject1->z = 1.6;
graspobject1->w = 1.9;
/* o9.b2->name = "Ball";
o9.b2->size = {1.3, 2.1, 3.2};
o9.b2->p1 = {12.5, 16.4, 17.3};
o9.b2->x = 1.1;
o9.b2->y = 1.4;
o9.b2->z = 1.6;
o9.b2->w = 1.9;
ros::Publisher chatter_pub = namespace_name.advertise<std_msgs::String>("chatter", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok()) {
std_msgs::UInt32 msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data.c_str());
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}
ros::Subscriber sub = namespace_name.subscribe("chatter", 1000, updateDectectedHumans);
*/
ros::Subscriber sub = namespace_name.subscribe("updateDectectedHumans", 1000, pub);
o9.humans->humannum = sub;
//o9.humans->humannum = 1;
ros::spin();
if(o9.humans->humannum>0)
ROS_INFO_STREAM("THE ROBOT IS IN AN UNSAFE STATE");
if(o9.humans->humannum==0)
ROS_INFO_STREAM("THE ROBOT IS IN A SAFE STATE");
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
......@@ -129,8 +140,7 @@ int main(int argc, char **argv) {
geometry_msgs::TransformStamped transformStamped5;
geometry_msgs::TransformStamped transformStamped6;
geometry_msgs::TransformStamped transformStamped7;
UInt32::data updateDetectedHumans;
geometry_msgs::UInt32 x;
try {
transformStamped1 = tfBuffer.lookupTransform("world", robot1.jo[1].name, ros::Time(0));
transformStamped2 = tfBuffer.lookupTransform("world", robot1.jo[2].name, ros::Time(0));
......@@ -139,7 +149,6 @@ int main(int argc, char **argv) {
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());
ros::Duration(0.1).sleep();
......
src/talker.cpp 0 → 100644
+ 36
− 0
View file @ fa38e8e1
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include "tf2_ros/message_filter.h"
#include "message_filters/subscriber.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/PointStamped.h"
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <std_msgs/UInt32.h>
#include "std_msgs/String.h"
#include <sstream>
int talker(int argc, char **argv) {
ros::init(argc, argv, "talker");
ros::NodeHandle n;
ros::Publisher chatter_pub = n.advertise<std_msgs::UInt32>("updateDectectedHumans", 1000);
ros::Rate loop_rate(10);
int count = 0;
while (ros::ok()) {
std_msgs::UInt32 msg;
std::stringstream ss;
ss << "hello world " << count;
msg.data = ss.str();
ROS_INFO("%s", msg.data);
chatter_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
++count;
}return 0;
}
src/worldsafety.cpp
+ 237
− 118
View file @ fa38e8e1
......@@ -32,12 +32,131 @@
#include "trajectory_msgs/JointTrajectoryPoint.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "Safety_Model");
ros::init(argc, argv, "WM_TEST_NODE");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO("WELCOME TO SAFETY MODEL TO CHECK IF ROBOT MOVED TO ITS POSITION");
ROS_INFO("WELCOME TO TEST NODE ");
// initialize the world
world world1;
auto robot1 = new robotfpe;
world1.c = robot1;
world1.humans = new humanspace;
world1.humans->humannum = 1;
auto *obstacle1 = new obstacle{};
obstacle1->name = "Cube";
// TODO set other properties of obstacle1
obstacle1->size = {5.2, 7.5, 9.1};
obstacle1->p1 = {13.2, 14.1, 17.5};
obstacle1->x = 2.3;
obstacle1->y = 1.2;
obstacle1->z = 0.2;
obstacle1->w = 0.1;
world1.objects.push_back(obstacle1);
auto *graspobject1 = new graspobject;
// TODO set the properties of the grasp object
graspobject1->name = "Ball";
graspobject1->size = {1.3, 2.1, 3.2};
graspobject1->p1 = {12.5, 16.4, 17.3};
graspobject1->x = 1.1;
graspobject1->y = 1.4;
graspobject1->z = 1.6;
graspobject1->w = 1.9;
world1.objects.push_back(graspobject1);
// initialize the robot
for (int i = 0; i < 7; i++) {
robot1->jo.emplace_back();
// we want the positions of the joints, but have the position of the links. there are 8 links, but 7 joints between
// them. So the first joint has the position of the second link etc.
robot1->jo[i].name = "panda_link" + std::to_string(i + 1);
}
robot1->h = new hand{};
// TODO set properties of robot1->h
robot1->h->grippos = {12.1, 14.3, 18.2};
// TODO set other properties of robot1
robot1->of = true;
robot1->moving = false;
robot1->grasppos = true;
robot1->planninggrp = "Panda Teaching";
// TODO test if the robot contains 7 joints
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 = " << robot1->jo.size()); }
// TODO test if the world contains two objects
if (world1.objects.size() == 2) { ROS_INFO_STREAM(" TEST CASE SUCCESSFUL. WORLD CONTAINS TWO OBJECTS "); }
else { ROS_ERROR_STREAM(" TEST CASE FAILED. WORLD HAS WRONG NUMBER OF OBJECTS = " << world1.objects.size()); }
// TODO perform nullptr checks
if (robot1->h == nullptr)
ROS_ERROR("ERROR IN HAND POINTER IN ROBOT FPE CLASS AGGREGATION RELATION");
if (world1.humans == nullptr)
ROS_ERROR("ERROR IN POINTER WORLD HUMANSPACE AGGREGATION");
if (world1.c == nullptr)
ROS_ERROR("ERROR IN ROBOT CLASS AGGREGATION");
if (world1.objects[0] == nullptr)
ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
if (world1.objects[1] == nullptr)
ROS_ERROR("ERROR IN ABSTRACT OBJECT CLASS AGGREGATION RELATION");
// TODO set another_pose
// TODO move the robot to another_pose
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();
//ros::shutdown();
// End Robot Motion
// Start input values from Gazebo
// ros::Duration(3.0).sleep();
// geometry_msgs::Pose another_pose;
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10);
while (node_handle.ok()) {
rate.sleep(); // if we sleep at the beginning of the loop,
// the first lookup also succeeds
// at this point we have checked the size of jo, so we can assume
// it is correct, thus we can iterate over jo
for (auto &i : robot1->jo) {
try {
auto transformStamped = tfBuffer.lookupTransform("world", i.name, ros::Time(0));
i.p1[0] = transformStamped.transform.translation.x;
// TODO set other parts of pose
i.p1[1] = transformStamped.transform.translation.y;
i.p1[2] = transformStamped.transform.translation.z;
} catch (tf2::TransformException &ex) {
ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep();
}
}
// dynamic tests
// test x and y position
double epsilon = 0.001; // 1mm deviation is allowed
double dev_x = 0; // TODO compute dev_x/y using std::abs, the position in
double dev_y = 0; // the model and another_pose.position.x/y
std::abs(dev_x/dev_y);
std::abs(another_pose.position.x/another_pose.position.y);
if (false) { // TODO add correct condition
ROS_INFO_STREAM(" ROBOT MOVED TO INTENDED POSITION. x/y deviations are " << dev_x << "/" << dev_y);
break; // the test succeeded, we can exit the loop
} else {
ROS_ERROR_STREAM("ROBOT DID NOT MOVE TO INTENDED POSITION. x/y deviations are " << dev_x << "/" << dev_y);
}
}
return 0;
}
/*
abstractobject o1;
graspobject o2;
hand o3;
......@@ -186,4 +305,4 @@ int main(int argc, char **argv) {
}
return 0;
}
}*/
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment