Go to the documentation of this file.00001 #ifdef DOXYGEN_SHOULD_SKIP_THIS
00002
00021 #endif // DOXYGEN_SHOULD_SKIP_THIS
00022
00023 #include <string>
00024 #include <stdio.h>
00025 #include <execinfo.h>
00026 #include <signal.h>
00027 #include <stdlib.h>
00028 #include <unistd.h>
00029 #include <ros/ros.h>
00030
00031 #include <grasp_planning_graspit_ros/WriteToFile.h>
00032 #include <grasp_planning_graspit_ros/LogBindingROS.h>
00033 #include <moveit_msgs/Grasp.h>
00034
00038 void print_trace(void)
00039 {
00040 void *array[10];
00041 size_t size;
00042 char **strings;
00043 size_t i;
00044
00045 size = backtrace(array, 10);
00046 strings = backtrace_symbols(array, size);
00047
00048 printf("Obtained %zd stack frames.\n", size);
00049
00050 for (i = 0; i < size; i++)
00051 printf("%s\n", strings[i]);
00052
00053 free(strings);
00054 }
00055
00056
00057 void handler(int sig)
00058 {
00059 print_trace();
00060 ros::shutdown();
00061 exit(1);
00062 }
00063
00064 int main(int argc, char **argv)
00065 {
00066 signal(SIGSEGV, handler);
00067 signal(SIGABRT, handler);
00068
00069 ros::init(argc, argv, "database_test", ros::init_options::AnonymousName);
00070
00071 bool useRosLogging = true;
00072 if (useRosLogging)
00073 {
00074 PRINT_INIT_ROS();
00075 }
00076 else
00077 {
00078 PRINT_INIT_STD();
00079 }
00080
00081 if (argc < 2)
00082 {
00083 PRINTERROR("Require a directory as command line argument");
00084 return 0;
00085 }
00086
00087 std::string output_path = std::string(argv[1]);
00088 if (!grasp_planning_graspit_ros::makeDirectoryIfNeeded(output_path))
00089 {
00090 PRINTERROR("Could not create directory "<<output_path);
00091 return 0;
00092 }
00093
00094 trajectory_msgs::JointTrajectory ttemp;
00095 ttemp.joint_names.push_back("jaco_finger_joint_0");
00096 ttemp.joint_names.push_back("jaco_finger_joint_2");
00097 ttemp.joint_names.push_back("jaco_finger_joint_4");
00098 trajectory_msgs::JointTrajectoryPoint jpre;
00099 jpre.positions.resize(3, 0.0045);
00100 trajectory_msgs::JointTrajectoryPoint jpost;
00101 jpost.positions.resize(3, -0.432062);
00102
00103 moveit_msgs::Grasp g1;
00104 g1.id = "jaco_robot_small_cube_0";
00105 g1.pre_grasp_posture = ttemp;
00106 g1.pre_grasp_posture.points.push_back(jpre);
00107 g1.grasp_posture = ttemp;
00108 g1.grasp_posture.points.push_back(jpost);
00109 g1.grasp_pose.pose.position.x = -0.0402668;
00110 g1.grasp_pose.pose.position.y = -0.0304912;
00111 g1.grasp_pose.pose.position.z = 0.185728;
00112 g1.grasp_pose.pose.orientation.x = -0.36809;
00113 g1.grasp_pose.pose.orientation.y = -0.609471;
00114 g1.grasp_pose.pose.orientation.z = -0.514462;
00115 g1.grasp_pose.pose.orientation.w = 0.477894;
00116 g1.grasp_quality = 32.3708;
00117 g1.max_contact_force = -1;
00118 if (!grasp_planning_graspit_ros::writeGraspMessage(g1, output_path, "Grasp_1"))
00119 {
00120 PRINTERROR("Could not save to file ");
00121 }
00122
00123 jpost.positions.resize(3, -0.443135);
00124 moveit_msgs::Grasp g2;
00125 g2.id = "jaco_robot_small_cube_1";
00126 g2.pre_grasp_posture = ttemp;
00127 g2.pre_grasp_posture.points.push_back(jpre);
00128 g2.grasp_posture = ttemp;
00129 g2.grasp_posture.points.push_back(jpost);
00130 g2.grasp_pose.pose.position.x = -0.0335148;
00131 g2.grasp_pose.pose.position.y = -0.0364662;
00132 g2.grasp_pose.pose.position.z = 0.188197;
00133 g2.grasp_pose.pose.orientation.x = -0.375924;
00134 g2.grasp_pose.pose.orientation.y = -0.584158;
00135 g2.grasp_pose.pose.orientation.z = -0.5217;
00136 g2.grasp_pose.pose.orientation.w = 0.495248;
00137 g2.grasp_quality = 32.4071;
00138 g2.max_contact_force = -1;
00139 if (!grasp_planning_graspit_ros::writeGraspMessage(g2, output_path, "Grasp_2"))
00140 {
00141 PRINTERROR("Could not save to file ");
00142 }
00143 return 0;
00144 }