merging of codes

catkin_ws/src/poppy_ros/CMakeLists.txt

@@ -12,7 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-  message_generation
+#  message_generation
 )
 find_package(OpenCV REQUIRED COMPONENTS
   core
@@ -65,10 +65,10 @@ find_package(OpenCV REQUIRED COMPONENTS
 #   Service1.srv
 #   Service2.srv
 # )
-add_service_files(
+#add_service_files(
-   FILES
+#   FILES
-   ik.srv
+#   ik.srv
-)
+#)
 ## Generate actions in the 'action' folder
 # add_action_files(
 #   FILES
@@ -81,10 +81,10 @@ add_service_files(
 #   DEPENDENCIES
 #   std_msgs
 # )
-generate_messages(
+#generate_messages(
-   DEPENDENCIES
+#   DEPENDENCIES
-   std_msgs
+#   std_msgs
-)
+#)
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
 ################################################
@@ -157,9 +157,9 @@ add_executable(poppy_test src/poppy_test.cpp)
 add_executable(poppy_pong_iv src/poppy_pong_iv.cpp)
 add_executable(poppy_track_x src/poppy_track_x.cpp)
 
-add_executable(ik_server src/ik_server.cpp /home/lucas/toolkit-kinematics/src/Kinematics.cpp /home/lucas/toolkit-kinematics/src/RotationMatrix.cpp /home/lucas/toolkit-kinematics/src/TranslationMatrix.cpp /home/lucas/toolkit-kinematics/src/TransformationMatrix.cpp)
+#add_executable(ik_server src/ik_server.cpp /home/lucas/toolkit-kinematics/src/Kinematics.cpp /home/lucas/toolkit-kinematics/src/RotationMatrix.cpp /home/lucas/toolkit-kinematics/src/TranslationMatrix.cpp /home/lucas/toolkit-kinematics/src/TransformationMatrix.cpp)
 
-add_executable(ik_client src/ik_client.cpp)
+#add_executable(ik_client src/ik_client.cpp)
 
 add_executable(sim_poppy_joint src/sim_poppy_joint.cpp)
 ## Rename C++ executable without prefix
@@ -181,9 +181,9 @@ target_link_libraries(poppy_test ${catkin_LIBRARIES})
 target_link_libraries(poppy_pong_iv ${catkin_LIBRARIES})
 target_link_libraries(poppy_track_x ${catkin_LIBRARIES})
 
-target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
+#target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
 
-target_link_libraries(ik_client ${catkin_LIBRARIES})
+#target_link_libraries(ik_client ${catkin_LIBRARIES})
 target_link_libraries(sim_poppy_joint ${catkin_LIBRARIES})
 #############
 ## Install ##

catkin_ws/src/poppy_ros/package.xml

@@ -52,11 +52,9 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
-  <build_depend>message_generation</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
-  <exec_depend>message_runtime</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>

catkin_ws/src/poppy_ros/src/ik_client.cpp

@@ -1,35 +0,0 @@
-#include <iostream>
-#include "ros/ros.h"
-#include "poppy_ros/ik.h"
-#include "geometry_msgs/Twist.h"
-
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "ik_client");
-	ros::NodeHandle nh;
-	ros::ServiceClient client = nh.serviceClient<poppy_ros::ik>("ik");
-	ros::Publisher joint_cmd_pub = nh.advertise<geometry_msgs::Twist>("/joint_cmd", 1);
-	poppy_ros::ik srv;
-	srv.request.x = atoll(argv[1]);
-	srv.request.y = atoll(argv[2]);
-	srv.request.z = atoll(argv[3]);
-	if (client.call(srv))
-	{
-		// Assuming joint values are to be published as a geometry_msgs::Twist message
-		geometry_msgs::Twist joint_cmd;
-		
-		joint_cmd.linear.x = srv.response.joints[0];
-		joint_cmd.linear.y = srv.response.joints[1];
-		joint_cmd.linear.z = srv.response.joints[2];
-		joint_cmd.angular.x = srv.response.joints[3];
-		joint_cmd.angular.y = srv.response.joints[4];
-		joint_cmd.angular.z = srv.response.joints[5];
-
-		joint_cmd_pub.publish(joint_cmd);
-		ROS_INFO("Command passed");
-	    } else {
-		ROS_ERROR("Failed to call service ik_service");
-	}
-	return 0;
-}

catkin_ws/src/poppy_ros/src/ik_server.cpp

@@ -1,49 +0,0 @@
-#include "ros/ros.h"
-#include "poppy_ros/ik.h"
-#include "Kinematics.h"
-#include <opencv2/opencv.hpp>
-
-Kinematics kinematics;
-
-bool ProcessIK(poppy_ros::ik::Request &req, poppy_ros::ik::Response &res) {
-    // Create a cv::Mat for Cartesian coordinates
-    cv::Mat cartesian_coords = (cv::Mat_<double>(3, 1) << req.x, req.y, req.z);
-
-    // Create a cv::Mat for joint values
-    cv::Mat joint_values;
-
-    // Perform inverse kinematics calculations and get the result directly
-    kinematics.inverseKinematics(cartesian_coords);
-    
-    // Get the joint values after inverse kinematics
-    joint_values = kinematics.computeCurrentEndEffectorPosition();
-
-
-    // Set the response
-    if (!joint_values.empty()) {
-        // Convert cv::Mat to std::vector<double>
-        std::vector<double> joint_values_vector(joint_values.begin<double>(), joint_values.end<double>());
-
-        // Set the response
-        res.joints = joint_values_vector;
-        return true;
-    } else {
-        ROS_ERROR("Inverse kinematics calculation failed.");
-        return false;
-    }
-}
-
-int main(int argc, char** argv)
-{
-	ros::init(argc, argv, "ik_server");
-	ros::NodeHandle nh;
-	
-	//load to the kinematics lib
-	kinematics.loadDHParameters("/home/lucas/catkin_ws/src/poppy_ros/src/poppy_dh.yaml");
-	kinematics.loadIKParameters("/home/lucas/catkin_ws/src/poppy_ros/src/poppy_ik.yaml");
-	
-	ros::ServiceServer server = nh.advertiseService("ik", ProcessIK);
-	ros::spin();
-	return 0;
-}
-

catkin_ws/src/poppy_ros/src/poppy_dh.yaml

@@ -1,11 +0,0 @@
-%YAML:1.0
----
-dhThetaParams: [ 0., 0., 0., 0., 0., 0., 0. ]
-dhDParams: [ 2.5000000000000000e+00, 2.5000000000000000e+00, 0., 0., 0.,
-    0., 0. ]
-dhAParams: [ 0., 0., 5.5000000000000000e+00, 3., 0., 5., 12. ]
-dhAlphaParams: [ 0., 90., 0., 0., -90., 90., 0. ]
-jointType: [ 0, 1, 1, 1, 0, 1, 1 ]
-nbJoints: 5
-jointOffset: [ 0., 0., 90., 0., -90., 0., 0. ]
-qiValues: [ 0., 0., 0., 0., 90. ]

catkin_ws/src/poppy_ros/src/poppy_ik.yaml

@@ -1,6 +0,0 @@
-%YAML:1.0
----
-maxNbIterations: 1000
-pController: 0.5
-rankThreshold: 0.2
-distanceThreshold: 0.01

catkin_ws/src/poppy_ros/srv/ik.srv

@@ -1,5 +0,0 @@
-int64 x
-int64 y
-int64 z
----
-float64[] joints