jaco buffer fix

mobiman_simulation/include/mobiman_simulation/jaco_utility.h

@@ -1,5 +1,6 @@
 #include <ros/ros.h>
 #include <kinova_msgs/JointVelocity.h>
+#include <kinova_msgs/ClearTrajectories.h>
 #include <trajectory_msgs/JointTrajectory.h>
 #include <iostream>
 #include <sensor_msgs/JointState.h>
@@ -39,14 +40,14 @@ Eigen::VectorXd propotional(joints_);
 Eigen::VectorXd derivative(joints_);
 Eigen::VectorXd integral(joints_);
 //PID Parameters
-double p = 27.0;
-double i = 0.2;
+double p = 100.0;
+double i = 0.3;
 double d = 0;
-double dt = 20;
+double dt = 100;
 // Kinova_velocity msg
 kinova_msgs::JointVelocity jaco_velocity;
-double max_ = 18.0;
-double min_ = -18.0;
+double max_ = 35.0;
+double min_ = -35.0;
 ros::Publisher velocity_publisher;
 void position_listener(trajectory_msgs::JointTrajectory trajectory);
 void jaco_feedback(sensor_msgs::JointState joint_state);
@@ -65,4 +66,7 @@ std::vector<std::vector<double>> data_target_velocity_;
 std::vector<std::vector<double>> data_pid_velocity_;
 std::vector<double> data_time_;
 std::string file_name;
-std::string file_path = "/home/alpharomeo911/datatset";
+std::string file_path = "/home/alpharomeo911/datatset";
+kinova_msgs::JointVelocity target_vel;
+kinova_msgs::ClearTrajectories clear_trajectory;
+ros::ServiceClient clear_trajectory_service;

mobiman_simulation/src/jaco_utility.cpp

@@ -10,6 +10,7 @@ int main(int argc, char **argv) {
     ros::Subscriber state_subscriber = nh.subscribe("/j2n6s300_driver/out/joint_state", 5, jaco_feedback);
     ros::Subscriber trajectory_subscriber = nh.subscribe("/arm_controller/command", 5, position_listener);
     ros::Subscriber velocity_subscriber = nh.subscribe("/arm_controller/velocity", 5, velocity_listener);
+    clear_trajectory_service = nh.serviceClient<kinova_msgs::ClearTrajectories>("/j2n6s300_driver/in/clear_trajectories");
     velocity_publisher = nh.advertise<kinova_msgs::JointVelocity>("/j2n6s300_driver/in/joint_velocity", 100);
 
     data_start_time = ros::Time::now();
@@ -30,6 +31,11 @@ int main(int argc, char **argv) {
 
 void position_listener(trajectory_msgs::JointTrajectory trajectory) {
     // jaco_trajectory = trajectory;
+    if(clear_trajectory_service.call(clear_trajectory)) {
+        std::cout << "[+] Trajectory Cleared" << std::endl;
+    } else {
+        std::cout << "[+] Trajectory not Cleared" << std::endl;
+    }
     mrt_target = Eigen::Map<Eigen::Matrix<double, 6, 1>>(trajectory.points[0].positions.data());
     start_pid = true;
     time_ = ros::Time::now();
@@ -132,6 +138,7 @@ void write_data(void) {
 
 void velocity_listener(kinova_msgs::JointVelocity target_velocity) {
     // target_joint_velocity.clear();
+    target_vel = target_velocity;
     target_joint_velocity[0] = target_velocity.joint1;
     target_joint_velocity[1] = target_velocity.joint2;
     target_joint_velocity[2] = target_velocity.joint3;