add gripper control in the mra_basic package

Author: MengNan-Li

.gitignore

@@ -0,0 +1,7 @@
+CMakeLists.txt.user
+qtbuild/
+.idea
+.idea/
+sender
+libIKOptimalHandler.so
+.DS_Store

mra_basic/include/mra_basic/config.h

@@ -8,6 +8,7 @@ namespace mra_basic_config {
 //const std::string DEFAULT_NODE = "/dev/pcanusb32"; //use:DEFAULT_NODE.c_str()
 #define DEFAULT_NODE "/dev/pcanusb32"
 const std::vector<int> jointID{21,22,23,24,25,26,27};
+const int GRIPPER_ID = 18;
 const std::vector<std::string> joint_names{"Joint1","Joint2","Joint3","Joint4","Joint5","Joint6","Joint7"};
 
 #define CONTROL_RATE 100 //100HZ
@@ -18,7 +19,7 @@ const std::vector<std::string> joint_names{"Joint1","Joint2","Joint3","Joint4","
 #define JOINT_COMMAND_TOPIC "/mra/joint_command"    //-->sub joint control command
 #define JOINT_POSITION_COMMAND_TOPIC "/moveJ"    //-->sub joint control command
 #define RESET_MRA_API_TOPIC "/mra/reset_MRA_API"    //-->sub, When canbus interrupts, we need to reset MRA_API after reconnecting the usb-can.
-
+#define GRIPPER_COMMAND "/mra/gripper_command"
 
 /*Position controll topic*/
 #define JOINT1_POSITION_CONTROLLER "/mra7a/joint1_position_controller/command"

mra_basic/src/joint_control/joint_control.cpp

@@ -42,11 +42,13 @@
 #include <mra_core_msgs/AssemblyState.h>
 #include <mra_core_msgs/JointCommand.h>
 #include <std_msgs/Bool.h>
+#include <std_msgs/Int8.h>
 
 using namespace mra_basic_config;
 using namespace std;
 
 UserControlOnCan *userControlOnCan;
+Gripper *gripper;
 mra_core_msgs::AssemblyState mra_state;
 
 void show_ID_in_current_canbus()
@@ -103,6 +105,14 @@ void moveJ_callback(const std_msgs::Float32MultiArray::ConstPtr &msg)
     }
 }
 
+void gripper_command_callback(const std_msgs::Int8ConstPtr &msg){
+    if(gripper != NULL){
+        ROS_INFO("Gripper work on %d", msg->data);
+        gripper->setFingerTagOpenStatus(msg->data);
+    }
+
+}
+
 void MRA_API_INIT(const std_msgs::Bool &reset) {
 
     userControlOnCan = new UserControlOnCan();
@@ -118,6 +128,9 @@ void MRA_API_INIT(const std_msgs::Bool &reset) {
         userControlOnCan->controller.GetErrorText(s);
         ROS_ERROR("Can't Open the pcanusb32:%s",s.c_str());
     }
+    if(userControlOnCan->controller.allGripper.size() != 0){
+        gripper = userControlOnCan->findGripperID(mra_basic_config::GRIPPER_ID);
+    }
 }
 
 int main(int argc, char **argv)
@@ -149,6 +162,8 @@ int main(int argc, char **argv)
     ros::Publisher state_pub = n.advertise<mra_core_msgs::AssemblyState> (STATE_TOPIC, 1);
     /*sub reset */
     ros::Subscriber sub_reset_MRA_API = n.subscribe(RESET_MRA_API_TOPIC, 10, &MRA_API_INIT);
+    /*sub gripper command*/
+    ros::Subscriber sub_gripper_command = n.subscribe(GRIPPER_COMMAND, 1, &gripper_command_callback);
 
 
     sensor_msgs::JointState joint_state;