cleanup based on Kinovarobotics#408

yuzhongchun17 · Aug 12, 2024 · 0d1ab8e · 0d1ab8e
1 parent d400d6b
commit 0d1ab8e
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 40 deletions.
diff --git a/kinova_driver/include/kinova/KinovaTypes.h b/kinova_driver/include/kinova/KinovaTypes.h
@@ -468,6 +468,31 @@ struct AngularInfo
 		Actuator6 = 0.0f;
 		Actuator7 = 0.0f;
 	}
+
+	float& operator [](const int idx){
+		switch (idx)
+		{
+			case 0: return Actuator1;
+			case 1: return Actuator2;
+			case 2: return Actuator3;
+			case 3: return Actuator4;
+			case 4: return Actuator5;
+			case 5: return Actuator6;
+			case 6: return Actuator7;
+		}
+	}
+    float operator [](const int idx) const{
+		switch (idx)
+		{
+			case 0: return Actuator1;
+			case 1: return Actuator2;
+			case 2: return Actuator3;
+			case 3: return Actuator4;
+			case 4: return Actuator5;
+			case 5: return Actuator6;
+			case 6: return Actuator7;
+		}
+	}
 };
 
 /** @brief This data structure holds values in an cartesian control context.
@@ -2718,4 +2743,4 @@ enum ROBOT_TYPE
 	GENERIC_ROBOT = 254
 };
 
-#endif
+#endif
diff --git a/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h b/kinova_driver/include/kinova_driver/kinova_joint_trajectory_controller.h
@@ -1,7 +1,6 @@
 #ifndef KINOVA_JOINT_TRAJECTORY_CONTROLLER_H
 #define KINOVA_JOINT_TRAJECTORY_CONTROLLER_H
 
-
 #include <ros/ros.h>
 #include <control_msgs/FollowJointTrajectoryAction.h>
 #include <sensor_msgs/JointState.h>
@@ -26,64 +25,45 @@ class JointTrajectoryController
 private:
     ros::NodeHandle nh_;
 
-    ros::Subscriber sub_command_;
-    ros::Publisher pub_joint_feedback_;
-    ros::Publisher pub_joint_velocity_;
+    ros::Subscriber sub_command_; // command subscriber listening to 'trajectory_controller/command'
+    ros::Publisher pub_joint_feedback_; // feedback publisher sending to 'trajectory_controller/state'
+    ros::Publisher pub_joint_velocity_; // velocity publisher sending to 'in/joint_velocity'
 
-    ros::Time previous_pub_;
-    ros::Time time_pub_joint_vel_;
+    ros::Time time_pub_joint_vel_; // time of timer thread start to publish joint velocity command
 
-    ros::Timer timer_pub_joint_vel_;
+    ros::Timer timer_pub_joint_vel_; // timer to publish joint velocities
     boost::mutex terminate_thread_mutex_;
     boost::thread* thread_update_state_;
     bool terminate_thread_;
 
-    std_msgs::Duration time_from_start_;
-    sensor_msgs::JointState current_joint_state_;
-
-    KinovaComm kinova_comm_;
-    TrajectoryPoint kinova_traj_point_;
+    KinovaComm kinova_comm_; // Communication pipeline for the arm
 
-//    trajectory_msgs::JointTrajectory joint_traj_;
-//    trajectory_msgs::JointTrajectoryPoint joint_traj_point_;
-    std::string traj_frame_id_;
-    std::vector<trajectory_msgs::JointTrajectoryPoint> traj_command_points_;
-    control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_;
+    std::string traj_frame_id_; // origin of trajectory transforms
+    std::vector<trajectory_msgs::JointTrajectoryPoint> traj_command_points_; // trajectory points from most recent trajectory
+    control_msgs::FollowJointTrajectoryFeedback traj_feedback_msg_; // trajectory feedback message
 
     // stores the command to send to robot, in Kinova type (KinovaAngles)
-    std::vector<KinovaAngles> kinova_angle_command_;
-
-    uint number_joint_;
-    int traj_command_points_index_;
-    std::vector<std::string> joint_names_;
-    std::string prefix_;
-
-    struct Segment
-    {
-        double start_time;
-        double duration;
-        std::vector<double> positions;
-        std::vector<double> velocities;
-    };
+    std::vector<KinovaAngles> kinova_angle_command_; // intermediate command storage
 
+    uint number_joint_; // number of joints of the robot
+    int traj_command_points_index_; // current index in traj_command_points_, defined by time
+    std::vector<std::string> joint_names_; // names of the joints
+    std::string prefix_; // robot name prefix
+    const static int num_possible_joints = 7; // number of possible joints supported by the system
+    float current_velocity_command[num_possible_joints]; // storage array to keep calculated velocity commands
+    double remaining_motion_time[num_possible_joints]; // time of motion remaining for each joint during the last command
 
     // call back function when receive a trajectory command
     void commandCB(const trajectory_msgs::JointTrajectoryConstPtr &traj_msg);
 
     // reflash the robot state and publish the joint state: either by timer or thread
     void update_state(); // by thread
 
+    // regularly publish the velocity commands of the trajectory 
     void pub_joint_vel(const ros::TimerEvent&); // by timer
-    int test;
 
 };
 
-
-
-
-
-
 }
 
-
-#endif // KINOVA_JOINT_TRAJECTORY_CONTROLLER_H
+#endif // KINOVA_JOINT_TRAJECTORY_CONTROLLER_H