#include <ros/ros.h>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "convert.h"
using namespace UNITREE_LEGGED_SDK;
int main(int argc, char **argv)
{
ros::init(argc, argv, "example_walk_without_lcm");
std::cout << "WARNING: Control level is set to HIGH-level." << std::endl
<< "Make sure the robot is standing on the ground." << std::endl
<< "Press Enter to continue..." << std::endl;
std::cin.ignore();
ros::NodeHandle nh;
ros::Rate loop_rate(500);
long motiontime = 0;
unitree_legged_msgs::HighCmd high_cmd_ros;
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
while (ros::ok())
{
motiontime += 2;
high_cmd_ros.head[0] = 0xFE;
high_cmd_ros.head[1] = 0xEF;
high_cmd_ros.levelFlag = HIGHLEVEL;
high_cmd_ros.mode = 0;
high_cmd_ros.gaitType = 0;
high_cmd_ros.speedLevel = 0;
high_cmd_ros.footRaiseHeight = 0;
high_cmd_ros.bodyHeight = 0;
high_cmd_ros.euler[0] = 0;
high_cmd_ros.euler[1] = 0;
high_cmd_ros.euler[2] = 0;
high_cmd_ros.velocity[0] = 0.0f;
high_cmd_ros.velocity[1] = 0.0f;
high_cmd_ros.yawSpeed = 0.0f;
high_cmd_ros.reserve = 0;
if (motiontime > 0 && motiontime < 1000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[0] = -0.3;
}
if (motiontime > 1000 && motiontime < 2000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[0] = 0.3; //躯体头部向下,尾部向上
}
if (motiontime > 2000 && motiontime < 3000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[1] = -0.2;
}
if (motiontime > 3000 && motiontime < 4000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[1] = 0.2; //躯体向左
}
if (motiontime > 4000 && motiontime < 5000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[2] = -0.2;
}
if (motiontime > 5000 && motiontime < 6000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.euler[2] = 0.2;
}
if (motiontime > 6000 && motiontime < 7000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = -0.2;
}
if (motiontime > 7000 && motiontime < 8000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = 0.1;
}
if (motiontime > 8000 && motiontime < 9000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = 0.0;
}
# 卧倒模式
if (motiontime > 9000 && motiontime < 11000)
{
high_cmd_ros.mode = 5;
}
# 起立模式
if (motiontime > 11000 && motiontime < 13000)
{
high_cmd_ros.mode = 6;
}
// 空置模式
if (motiontime > 13000 && motiontime < 14000)
{
high_cmd_ros.mode = 0;
}
// 前进模式+旋转
if (motiontime > 14000 && motiontime < 18000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 2;
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1 前进速度
high_cmd_ros.yawSpeed = 2; //旋转速度
high_cmd_ros.footRaiseHeight = 0.1;
// printf("walk\n");
}
if (motiontime > 18000 && motiontime < 20000)
{
high_cmd_ros.mode = 0;
high_cmd_ros.velocity[0] = 0;
}
// 单纯的前进模式
if (motiontime > 20000 && motiontime < 24000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 1;
high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1
high_cmd_ros.bodyHeight = 0.1;
// printf("walk\n");
}
if (motiontime > 24000)
{
high_cmd_ros.mode = 1;
}
pub.publish(high_cmd_ros);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
Go1机器狗unitree_ros_to_real包example_walking代码分析
最新推荐文章于 2024-04-18 09:59:21 发布