Merge pull request #3 from MiRoboticsLab/v1.2.0.0

V1.2.0.0

elec_skin/CMakeLists.txt

@@ -0,0 +1,67 @@
+cmake_minimum_required(VERSION 3.8)
+project(elec_skin)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(toml REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+find_package(cyberdog_common REQUIRED)
+find_package(cyberdog_embed_protocol REQUIRED)
+
+include_directories(include)
+
+add_executable(${PROJECT_NAME} src/main.cpp)
+
+# MESSAGE('--------------${CMAKE_INSTALL_PREFIX}------------------')
+
+ament_target_dependencies( ${PROJECT_NAME}
+  rclcpp
+  std_msgs
+  toml
+  ament_index_cpp
+  cyberdog_common
+  cyberdog_embed_protocol )
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(TARGETS ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME})
+
+install(
+  DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY toml_config/
+  DESTINATION share/${PROJECT_NAME}/toml_config/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+
+ament_package()

elec_skin/include/elec_skin/elec_skin.hpp

@@ -0,0 +1,112 @@
+// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef ELEC_SKIN__ELEC_SKIN_HPP_
+#define ELEC_SKIN__ELEC_SKIN_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <map>
+#include "ament_index_cpp/get_package_share_directory.hpp"
+#include "embed_protocol/embed_protocol.hpp"
+#include "cyberdog_common/cyberdog_log.hpp"
+#include "elec_skin/elec_skin_base.hpp"
+
+#define EVM cyberdog::embed
+
+namespace cyberdog
+{
+namespace motion
+{
+
+typedef struct _can_data
+{
+  uint8_t data0;
+  uint8_t data1;
+  uint8_t data2;
+  uint8_t data3;
+} can_data;
+
+class ElecSkin final : public ElecSkinBase
+{
+public:
+  ElecSkin()
+  : ms_(ModelSwitch::MS_FLASH)
+  {
+    std::string path = ament_index_cpp::get_package_share_directory("elec_skin") +
+      "/toml_config/elec_skin.toml";
+    ptr_can_protocol_ = std::make_shared<EVM::Protocol<can_data>>(path, false);
+    ptr_can_protocol_->SetDataCallback(
+      std::bind(
+        &ElecSkin::recv_can_callback, this,
+        std::placeholders::_1, std::placeholders::_2));
+  }
+
+  ~ElecSkin()
+  {
+  }
+
+  void ModelControl(ModelSwitch ms, uint16_t milsecs)
+  {
+    ms_ = ms;
+    uint8_t wave_mode = static_cast<uint8_t>(ms_);
+    uint8_t wave_cycle_time_high = *(reinterpret_cast<uint8_t *>(&milsecs));
+    uint8_t wave_cycle_time_low = *(reinterpret_cast<uint8_t *>(&milsecs) + 1);
+    ptr_can_protocol_->Operate(
+      "model_on",
+      std::vector<uint8_t>{wave_mode, wave_cycle_time_low, wave_cycle_time_high});
+  }
+
+  void PositionContril(
+    PositionSkin ps, PositionColorChangeDirection pccd,
+    PositionColorStartDirection pcsd, uint16_t milsecs)
+  {
+    if (ms_ != ModelSwitch::MS_CONTROL) {
+      ModelControl(ModelSwitch::MS_CONTROL, milsecs);
+    }
+    uint8_t change_direction = static_cast<uint8_t>(pccd);
+    uint8_t start_direction = static_cast<uint8_t>(pcsd);
+    uint8_t wave_cycle_time_high = *(reinterpret_cast<uint8_t *>(&milsecs));
+    uint8_t wave_cycle_time_low = *(reinterpret_cast<uint8_t *>(&milsecs) + 1);
+    ptr_can_protocol_->Operate(
+      control_dog_action_map.at(ps),
+      std::vector<uint8_t>{change_direction, start_direction,
+        wave_cycle_time_low, wave_cycle_time_high});
+  }
+
+private:
+  void recv_can_callback(std::string & name, std::shared_ptr<can_data> data)
+  {
+    (void) name;
+    (void) data;
+  }
+
+private:
+  std::shared_ptr<EVM::Protocol<can_data>> ptr_can_protocol_;
+  ModelSwitch ms_;
+  const std::map<PositionSkin, std::string> control_dog_action_map = {
+    {PositionSkin::PS_BODYM, "body_middle"},
+    {PositionSkin::PS_LBLEG, "left_back_leg"},
+    {PositionSkin::PS_BODYL, "body_left"},
+    {PositionSkin::PS_LFLEG, "left_front_leg"},
+    {PositionSkin::PS_FRONT, "front_chest"},
+    {PositionSkin::PS_RFLEG, "right_front_leg"},
+    {PositionSkin::PS_BODYR, "body_right"},
+    {PositionSkin::PS_RBLEG, "right_back_leg"}
+  };
+};  // class ElecSkin
+}  // namespace motion
+}  // namespace cyberdog
+
+#endif  // ELEC_SKIN__ELEC_SKIN_HPP_

elec_skin/include/elec_skin/elec_skin_base.hpp

@@ -0,0 +1,69 @@
+// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef ELEC_SKIN__ELEC_SKIN_BASE_HPP_
+#define ELEC_SKIN__ELEC_SKIN_BASE_HPP_
+
+#include <stdint.h>
+
+namespace cyberdog
+{
+namespace motion
+{
+
+enum class ModelSwitch : uint8_t
+{
+  MS_FLASH = 0,    // 闪烁
+  MS_WAVEF = 1,    // 动画前向后变
+  MS_RANDOM = 2,   // 随机
+  MS_WAVEB = 3,    // 动画后向前变
+  MS_CONTROL = 4,  // 上位机实时控制模式
+};  // enum class PositionSkin
+
+enum class PositionSkin : uint8_t
+{
+  PS_BODYM = 0,  // 背部
+  PS_LBLEG = 1,  // 左后腿
+  PS_BODYL = 2,  // 左侧
+  PS_LFLEG = 3,  // 左前腿
+  PS_FRONT = 4,  // 前胸
+  PS_RFLEG = 5,  // 右前腿
+  PS_BODYR = 6,  // 右侧
+  PS_RBLEG = 7,  // 右后腿
+};  // enum class PositionSkin
+
+enum class PositionColorChangeDirection : uint8_t
+{
+  PCCD_WTOB = 1,   // 白变黑
+  PCCD_BTOW = 0,   // 黑变白
+};  // enum class PositionColorChangeDirection
+
+enum class PositionColorStartDirection : uint8_t
+{
+  PCSD_FRONT = 0,   // 前方先变色
+  PCSD_BACK  = 1,   // 后方先变色
+};  // enum class PositionColorStartDirection
+
+class ElecSkinBase
+{
+public:
+  virtual void ModelControl(ModelSwitch ms, uint16_t milsecs) = 0;
+  virtual void PositionContril(
+    PositionSkin ps, PositionColorChangeDirection pccd,
+    PositionColorStartDirection pcsd, uint16_t milsecs) = 0;
+};
+
+}  // namespace motion
+}  // namespace cyberdog
+
+#endif  // ELEC_SKIN__ELEC_SKIN_BASE_HPP_

elec_skin/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>elec_skin</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">liukai21</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>toml</depend>
+  <depend>ament_index_cpp</depend>
+  <depend>cyberdog_common</depend>
+  <depend>cyberdog_embed_protocol</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

elec_skin/src/main.cpp

@@ -0,0 +1,117 @@
+// Copyright (c) 2021 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <string>
+#include <memory>
+#include <vector>
+#include <map>
+#include "elec_skin/elec_skin_base.hpp"
+#include "elec_skin/elec_skin.hpp"
+#include "cyberdog_common/cyberdog_log.hpp"
+#include "std_msgs/msg/string.hpp"
+
+#define   NODE_NAME   "elec_skin_node_test"
+
+// 使用字符分割
+void string_split(
+  const std::string & str,
+  const char split, std::vector<std::string> & res)
+{
+  if (str == "") {
+    return;
+  }
+  // 在字符串末尾也加入分隔符，方便截取最后一段
+  std::string strs = str + split;
+  size_t pos = strs.find(split);
+  // 若找不到内容则字符串搜索函数返回 npos
+  while (pos != strs.npos) {
+    std::string temp = strs.substr(0, pos);
+    res.push_back(temp);
+    // 去掉已分割的字符串,在剩下的字符串中进行分割
+    strs = strs.substr(pos + 1, strs.size());
+    pos = strs.find(split);
+  }
+}
+
+class ElecSkinNode : public rclcpp::Node
+{
+public:
+  ElecSkinNode()
+  : Node(NODE_NAME)
+  {
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+      "elec_skin_test",
+      10,
+      [this](std_msgs::msg::String::SharedPtr msg) {
+        std::vector<std::string> str_vec;
+        string_split(msg->data, ',', str_vec);
+        if (str_vec.size() < 1) {
+          return;
+        }
+        uint8_t method = 255;
+        std::map<std::string, uint8_t> m_cmd {{"model", 0}, {"position", 1}};
+        if (m_cmd.find(str_vec[0]) == m_cmd.end()) {
+          return;
+        }
+        INFO("handle elec skin control:>>>");
+        method = m_cmd[str_vec[0]];
+        switch (method) {
+          case 0:
+            {
+              if (str_vec.size() != 3) {
+                INFO("parameter counts error, should be 3!");
+                return;
+              }
+              cyberdog::motion::ModelSwitch ms =
+              cyberdog::motion::ModelSwitch(atoi(str_vec[1].c_str()));
+              uint16_t milsecs = static_cast<uint16_t>(atoi(str_vec[2].c_str()));
+              can_control_->ModelControl(ms, milsecs);
+            }
+            break;
+          case 1:
+            {
+              if (str_vec.size() != 5) {
+                INFO("parameter counts error, should be 4!");
+                return;
+              }
+              cyberdog::motion::PositionSkin ps =
+              cyberdog::motion::PositionSkin(atoi(str_vec[1].c_str()));
+              cyberdog::motion::PositionColorChangeDirection pccd =
+              cyberdog::motion::PositionColorChangeDirection(atoi(str_vec[2].c_str()));
+              cyberdog::motion::PositionColorStartDirection pcsd =
+              cyberdog::motion::PositionColorStartDirection(atoi(str_vec[3].c_str()));
+              uint16_t milsecs = static_cast<uint16_t>(atoi(str_vec[4].c_str()));
+              can_control_->PositionContril(ps, pccd, pcsd, milsecs);
+            }
+            break;
+          default:
+            break;
+        }
+      });
+    can_control_ = std::make_unique<cyberdog::motion::ElecSkin>();
+  }
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+  std::unique_ptr<cyberdog::motion::ElecSkinBase> can_control_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  LOGGER_MAIN_INSTANCE("elec_skin");
+  rclcpp::spin(std::make_shared<ElecSkinNode>());
+  rclcpp::shutdown();
+  return 0;
+}