mani_test

wpb_home_bringup/CMakeLists.txt

@@ -232,7 +232,6 @@ target_link_libraries(wpb_home_lidar_filter
 )
 
 ## Declare a C++ executable
-
 add_executable(wpb_home_imu_tf
   src/wpb_home_imu_tf.cpp
 )
@@ -240,3 +239,14 @@ add_dependencies(wpb_home_imu_tf ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EX
 target_link_libraries(wpb_home_imu_tf
   ${catkin_LIBRARIES}
 )
+
+## Declare a C++ executable
+add_executable(wpb_home_test_mani
+  src/wpb_home_test_mani.cpp
+  src/driver/SerialCom.cpp
+  src/driver/WPB_Home_driver.cpp
+)
+add_dependencies(wpb_home_test_mani ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(wpb_home_test_mani
+  ${catkin_LIBRARIES}
+)

wpb_home_bringup/include/driver/WPB_Home_driver.h

@@ -47,6 +47,7 @@ public:
 	void Velocity(float inX, float inY, float inAngular);
 	void SendMotors(int inMotor1, int inMotor2, int inMotor3, int inMotor4);
 	float GetYaw();
+	void ManiCtrl(float inHeight, int inRaiseSpeed, float inGripper, int inGripperSpeed);
 	float fLinearAccLimit;
 	float fAngularAccLimit;
 
@@ -87,4 +88,6 @@ protected:
 	void m_ParseFrame();
 	void m_DisRecv();
 	int GenCmd(int inBuffOffset, int inDevID, int inModule, int inMethod, unsigned char* inData, int inDataLen);
+	void MotorCmd(int inMethod, int inID1, int inValue1, int inID2, int inValue2);
+	void MotorCmd2(int inMethod, int inID1, int inValue1_1, int inValue1_2, int inID2, int inValue2_1, int inValue2_2);
 };

wpb_home_bringup/launch/mani_test.launch

@@ -0,0 +1,8 @@
+<launch>
+
+  <!-- wpb_home mani test-->
+  <node pkg="wpb_home_bringup" type="wpb_home_test_mani" name="wpb_home_test_mani" output="screen">
+    <param name="serial_port" type="string" value="/dev/ftdi"/> 
+  </node>
+
+</launch>

wpb_home_bringup/src/driver/WPB_Home_driver.cpp

@@ -315,3 +315,44 @@ float CWPB_Home_driver::GetYaw()
 	float diffYaw = fCurYaw - fFirstYaw;
 	return diffYaw;
 }
+
+
+void CWPB_Home_driver::MotorCmd(int inMethod, int inID1, int inValue1, int inID2, int inValue2)
+{
+	static unsigned char arMotorSpeedData[12];
+	m_Split2Bytes(arMotorSpeedData,inID1);
+	m_Split4Bytes(arMotorSpeedData + 2, inValue1);
+
+	m_Split2Bytes(arMotorSpeedData + 6, inID2);
+	m_Split4Bytes(arMotorSpeedData + 8, inValue2);
+	int nCmdLenght = GenCmd(0, 0x40, 0x08, inMethod, arMotorSpeedData, 12);
+	Send(m_SendBuf, nCmdLenght);
+}
+
+
+void CWPB_Home_driver::MotorCmd2(int inMethod, int inID1, int inValue1_1, int inValue1_2, int inID2, int inValue2_1, int inValue2_2)
+{
+	static unsigned char arMotorCmdData[20];
+	m_Split2Bytes(arMotorCmdData, inID1);
+	m_Split4Bytes(arMotorCmdData + 2, inValue1_1);
+	m_Split4Bytes(arMotorCmdData + 6, inValue1_2);
+
+	m_Split2Bytes(arMotorCmdData + 10, inID2);
+	m_Split4Bytes(arMotorCmdData + 12, inValue2_1);
+	m_Split4Bytes(arMotorCmdData + 16, inValue2_2);
+	int nCmdLenght = GenCmd(0, 0x40, 0x08, inMethod, arMotorCmdData, 20);
+	Send(m_SendBuf, nCmdLenght);
+}
+
+void CWPB_Home_driver::ManiCtrl(float inHeight, int inRaiseSpeed, float inGripper, int inGripperSpeed)
+{
+	if(inHeight > 0.01)
+	{
+		MotorCmd2(0x63, 5, inRaiseSpeed, inHeight, 6, inGripperSpeed, inGripper);
+	}
+	else
+	{
+		// 折叠
+		MotorCmd2(0x64, 5, inRaiseSpeed, inHeight, 6, inGripperSpeed, inGripper);
+	}
+}

wpb_home_bringup/src/wpb_home_test_mani.cpp

@@ -0,0 +1,125 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the WaterPlus nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+/*!******************************************************************
+ @author     ZhangWanjie
+ ********************************************************************/
+
+#include <ros/ros.h>
+#include "driver/WPB_Home_driver.h"
+
+#define WPBH_TEST_MANI_ZERO 0
+#define WPBH_TEST_MANI_DOWN 1
+#define WPBH_TEST_MANI_UP   2
+#define WPBH_TEST_MANI_FOLD 3
+
+#define CMD_WAIT_SEC 6
+
+static int nState = WPBH_TEST_MANI_ZERO;
+static int nCount = 0;
+
+int main(int argc, char** argv)
+{
+    ros::init(argc,argv,"wpb_home_test_mani");
+    ros::NodeHandle n;
+
+    ROS_WARN("[TEST] Manipulator...");
+    CWPB_Home_driver m_wpb_home;
+    ros::NodeHandle n_param("~");
+    std::string strSerialPort;
+    n_param.param<std::string>("serial_port", strSerialPort, "/dev/ttyUSB0");
+    m_wpb_home.Open(strSerialPort.c_str(),115200);
+    
+    ros::Rate r(1);
+
+    while(ros::ok())
+    {
+        switch(nState)
+        { 
+        case WPBH_TEST_MANI_ZERO:   //收起状态,先变成初始状态
+            ROS_INFO("[Mani] ZERO -> DOWN... %d",nCount);
+            if (nCount == 0)
+            {
+                m_wpb_home.ManiCtrl( 11200,  1000, 50000, 1000);
+            }
+            nCount ++;
+            if(nCount >= CMD_WAIT_SEC)
+            {
+                nState = WPBH_TEST_MANI_DOWN;
+                nCount = 0;
+            }
+            break;
+        case WPBH_TEST_MANI_DOWN:
+            ROS_INFO("[Mani] DOWN -> UP ... %d",nCount);
+            if (nCount == 0)
+            {
+                m_wpb_home.ManiCtrl( 55000,  1000, 0, 1000);
+            }
+            nCount ++;
+            if(nCount >= CMD_WAIT_SEC)
+            {
+                nState = WPBH_TEST_MANI_UP;
+                nCount = 0;
+            }
+            break;
+        case WPBH_TEST_MANI_UP:
+             ROS_INFO("[Mani] UP -> DOWN ... %d",nCount);
+            if (nCount == 0)
+            {
+                m_wpb_home.ManiCtrl( 11200,  1000, 0, 1000);
+            }
+            nCount ++;
+            if(nCount >= CMD_WAIT_SEC)
+            {
+                nState = WPBH_TEST_MANI_FOLD;
+                nCount = 0;
+            }
+            break;
+         case WPBH_TEST_MANI_FOLD:
+             ROS_INFO("[Mani] DOWN -> ZERO ... %d",nCount);
+            if (nCount == 0)
+            {
+                m_wpb_home.ManiCtrl( 0,  1000, 0, 1000);
+            }
+            nCount ++;
+            if(nCount >= CMD_WAIT_SEC)
+            {
+                nState = WPBH_TEST_MANI_ZERO;
+                nCount = 0;
+            }
+            break;
+        }
+       
+        r.sleep();
+    }
+}

wpb_home_bringup/src/wpb_home_test_motors.cpp

@@ -51,9 +51,12 @@ int main(int argc, char** argv)
     ros::init(argc,argv,"wpb_home_test_motors");
     ros::NodeHandle n;
 
-    ROS_INFO("TEST Start...");
+    ROS_INFO("[TEST] Motors...");
     CWPB_Home_driver m_wpb_home;
-    m_wpb_home.Open("/dev/ttyUSB1",115200);
+   ros::NodeHandle n_param("~");
+    std::string strSerialPort;
+    n_param.param<std::string>("serial_port", strSerialPort, "/dev/ttyUSB0");
+    m_wpb_home.Open(strSerialPort.c_str(),115200);
     
     int nTest = TEST_READY;
     ros::Rate r(0.3);