lab 5 smooth rotate, course work, Итальянцев Ян, 3304

3304/italyantsev_yan/cw/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cw)
+
+set(CMAKE_CXX_FLAGS "-std=c++0x")
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    tf
+    std_msgs
+    gazebo_ros
+    message_generation
+)
+
+
+## Generate messages in the 'msg' folder
+ add_message_files(
+   FILES
+   status.msg
+ )
+
+## Generate added messages and services with any dependencies listed here
+ generate_messages(
+   DEPENDENCIES
+   std_msgs  # Or other packages containing msgs
+ )
+
+catkin_package(
+   #INCLUDE_DIRS include
+   #LIBRARIES cw
+   CATKIN_DEPENDS roscpp tf std_msgs gazebo_ros message_runtime
+   DEPENDS gazebo_ros
+)
+
+include_directories(include
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+  ${SDFormat_INCLUDE_DIRS}
+)
+
+
+add_executable(str src/supplies.cpp src/robot_info.cpp)
+target_link_libraries(str ${catkin_LIBRARIES})
+add_dependencies(str cw_generate_messages_cpp)
+
+add_executable(survivor src/survivor.cpp src/robot_info.cpp)
+target_link_libraries(survivor ${catkin_LIBRARIES})
+add_dependencies(survivor cw_generate_messages_cpp)
+
+##add_executable(bonus src/bonus.cpp src/bonus_info.cpp)
+##target_link_libraries(bonus ${catkin_LIBRARIES})
+##add_dependencies(bonus cw_generate_messages_cpp)
+
+add_executable(zombies src/zombies.cpp src/robot_info.cpp)
+target_link_libraries(zombies ${catkin_LIBRARIES})
+add_dependencies(zombies cw_generate_messages_cpp)
+#

3304/italyantsev_yan/cw/cw.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node pkg="cw" type="zombies" name="zombie" output="screen"/>
+  <node pkg="cw" type="str" name="supplies" output="screen"/>
+</launch>

3304/italyantsev_yan/cw/msg/status.msg

@@ -0,0 +1,8 @@
+bool is_hooked
+bool check1
+bool check2
+bool check3
+bool check4
+bool check5
+bool exit
+

3304/italyantsev_yan/cw/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<package format="1">
+  <name>cw</name>
+  <version>0.0.0</version>
+  <description>The cw package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="bedrang@todo.todo">bedrang</maintainer>
+
+  <license>TODO</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>gazebo_ros</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+  <export>
+
+  </export>
+</package>

3304/italyantsev_yan/cw/src/robot_info.cpp

@@ -0,0 +1,111 @@
+#include <cmath>
+#include "robot_info.h"
+#include "ros/ros.h"
+
+RobotInfo::RobotInfo(const std::string& name) : RobotInfo(name, 0.01, 1.0 * M_PI / 180, 0.0, 1.0, 0.1, 0.05)
+{
+}
+
+RobotInfo::RobotInfo(const std::string& name, double distancePrecision, double angularPrecision, double currentAngle,
+          double linearSpeed, double angularSpeed, double maxLinearSpeed)
+    : mDistancePrecision(distancePrecision), mAngularPrecision(angularPrecision), mCurrentAngle(currentAngle),
+      mLinearSpeed(linearSpeed), mAngularSpeed(angularSpeed),mMaxLinearSpeed(maxLinearSpeed),
+      mX(0.0), mY(0.0), mName(name)
+{
+}
+
+void RobotInfo::setPosition(double x, double y)
+{
+    mX = x;
+    mY = y;
+}
+
+double RobotInfo::getDistancePrecision()
+{
+    return mDistancePrecision;
+}
+
+double RobotInfo::getAngularPrecision()
+{
+    return mAngularPrecision;
+}
+
+double RobotInfo::getCurrentAngle()
+{
+    return mCurrentAngle;
+}
+
+double RobotInfo::getLinearSpeed()
+{
+    return mLinearSpeed;
+}
+
+double RobotInfo::getAngularSpeed()
+{
+    return mAngularSpeed;
+}
+
+double RobotInfo::getMaxLinearSpeed()
+{
+    return mMaxLinearSpeed;
+}
+
+double RobotInfo::getX()
+{
+    return mX;
+}
+
+double RobotInfo::getY()
+{
+    return mY;
+}
+
+std::string RobotInfo::getName()
+{
+    return mName;
+}
+
+void RobotInfo::updatePosition(double dx, double dy, double distance)
+{
+    // update coords
+    double currentLinearSpeed = mLinearSpeed * distance;
+    if (currentLinearSpeed > mMaxLinearSpeed) currentLinearSpeed = mMaxLinearSpeed;
+    double diffAngle = atan2(dy, dx);
+    mX += currentLinearSpeed * std::cos(diffAngle);
+    mY += currentLinearSpeed * std::sin(diffAngle);
+
+    // update angle
+    ros::Rate r(30);
+    diffAngle = getRotateAngle(dx, dy);
+    float da = M_PI / 20;
+    float dt = (float)r.expectedCycleTime().toSec();
+    if (fabs(mCurrentAngle - diffAngle) < 2 * da)
+        {
+            dx += mX * dt;
+            dy += mY * dt;
+            mCurrentAngle = diffAngle;
+        }
+        else
+        {
+            if (diffAngle > mCurrentAngle)
+            {
+                mCurrentAngle = (mCurrentAngle + da > M_PI ? 0 : mCurrentAngle + da);
+            }
+            else
+            {
+                mCurrentAngle = (mCurrentAngle - da < -M_PI ? 0 : mCurrentAngle - da);
+            }
+        }
+}
+
+double RobotInfo::getRotateAngle(double dx, double dy)
+{
+    if (dy == 0) return (dx < 0 ? M_PI : 0.0);
+    else
+    {
+        if (dx == 0) return (dy < 0 ? -M_PI/2 : M_PI/2);
+        double angleCos = dx / std::sqrt(dx * dx + dy * dy);
+        return (dy < 0 ? -std::acos(angleCos) : std::acos(angleCos));
+    }
+    return 0.0;
+} 

3304/italyantsev_yan/cw/src/robot_info.h

@@ -0,0 +1,39 @@
+#ifndef ROBOT_INFO_H
+#define ROBOT_INFO_H
+
+#include <string>
+
+class RobotInfo
+{
+public:
+    RobotInfo(const std::string& name);
+    RobotInfo(const std::string& name, double distancePrecision, double angularPrecision, double currentAngle,
+              double linearSpeed, double angularSpeed, double maxLinearSpeed);
+    void setPosition(double x, double y);
+    double getDistancePrecision();
+    double getAngularPrecision();
+    double getCurrentAngle();
+    double getLinearSpeed();
+    double getAngularSpeed();
+    double getMaxLinearSpeed();
+    double getX();
+    double getY();
+    std::string getName();
+    void updatePosition(double dx, double dy, double distance);
+
+private:
+    double getRotateAngle(double dx, double dy);
+
+private:
+    double mDistancePrecision;
+    double mAngularPrecision;
+    double mCurrentAngle;
+    double mLinearSpeed;
+    double mAngularSpeed;
+    double mMaxLinearSpeed;
+    double mX;
+    double mY;
+    std::string mName;
+};
+
+#endif // ROBOT_INFO_H