Mbot look while walking

mas_common_robotics/mcr_common/mcr_common_converters/CMakeLists.txt

@@ -55,6 +55,17 @@ target_link_libraries(twist_to_motion_direction_conversion_node
 add_dependencies(twist_to_motion_direction_conversion_node ${catkin_EXPORTED_TARGETS})
 
 
+add_executable(mbot_looking_while_walking
+  ros/src/mbot_looking_while_walking.cpp
+)
+target_link_libraries(mbot_looking_while_walking
+  ${catkin_LIBRARIES}
+  motion_direction_calculation
+)
+add_dependencies(mbot_looking_while_walking ${catkin_EXPORTED_TARGETS})
+
+
+
 ### TESTS
 roslint_cpp()
 roslint_python(
@@ -85,6 +96,13 @@ install(TARGETS motion_direction_calculation twist_to_motion_direction_conversio
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+
+install(TARGETS motion_direction_calculation mbot_looking_while_walking
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 install(DIRECTORY ros/include/${PROJECT_NAME}/
   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
   FILES_MATCHING PATTERN "*.h"

mas_common_robotics/mcr_common/mcr_common_converters/ros/config/mbot_looking_while_walking.yaml

@@ -0,0 +1,11 @@
+# COPIED FROM ROBOCUP-AT-WORK
+
+frame_id: /head_link        
+distance_to_frame: 1.5
+
+# HEAD LIMITS     
+                  # Note:   0 degrees  mbot looks front
+min_angle: 5      # degrees
+max_angle: 175    # degrees
+
+# DYNAMIC RECONFIGURE

mas_common_robotics/mcr_common/mcr_common_converters/ros/config/mbot_twist_to_motion_direction_conversion.yaml

@@ -0,0 +1,2 @@
+frame_id: /head_link
+distance_to_frame: 1.5

mas_common_robotics/mcr_common/mcr_common_converters/ros/include/mcr_common_converters/twist_to_motion_direction_conversion_node.h

@@ -14,6 +14,8 @@
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <std_msgs/String.h>
+#include <std_msgs/UInt8MultiArray.h>
+#include <sensor_msgs/JointState.h>
 #include <tf/transform_datatypes.h>
 #include <string>
 
@@ -33,15 +35,21 @@ class TwistToMotionDirectionConversionNode
 private:
     void twistCallback(const geometry_msgs::TwistPtr &msg);
     void eventCallback(const std_msgs::StringPtr &msg);
+    // Turning head while walking feature
+    void headCallback(const sensor_msgs::JointStatePtr &msg);
 
     void computeMotionDirectionAndPublish();
 
 
     ros::Subscriber sub_twist_;
     ros::Subscriber sub_event_;
+    // Turning head while walking feature
+    ros::Subscriber sub_head_position_;
 
     ros::Publisher pub_pose_;
     ros::Publisher pub_point_;
+    // Turning head while walking feature
+    ros::Publisher pub_head_;
 
     std::string frame_id_;
     double distance_to_frame_;
@@ -52,6 +60,9 @@ class TwistToMotionDirectionConversionNode
     std_msgs::String event_msg_;
     bool event_msg_received_;
 
+    // Turning head while walking feature
+    sensor_msgs::JointState head_msg_;
+
     State current_state_;
 };
 

mas_common_robotics/mcr_common/mcr_common_converters/ros/launch/mbot_looking_while_walking.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="input_topic" default="/base/twist_controller/command" />
+
+  <node pkg="mcr_common_converters" type="mbot_looking_while_walking" name="mbot_looking_while_walking" ns="mcr_common" output="screen">
+    <rosparam command="load" file="$(find mcr_common_converters)/ros/config/twist_to_motion_direction_conversion.yaml" />
+    <remap from="~/input/twist" to="$(arg input_topic)" />
+  </node>
+
+</launch>
+

mas_common_robotics/mcr_common/mcr_common_converters/ros/launch/mbot_twist_to_motion_direction_conversion.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<launch>
+  <arg name="input_topic" default="/base/twist_controller/command" />
+
+  <node pkg="mcr_common_converters" type="mbot_looking_while_walking" name="mbot_looking_while_walking" ns="mcr_common" output="screen">
+    <rosparam command="load" file="$(find mcr_common_converters)/ros/config/twist_to_motion_direction_conversion.yaml" />
+    <remap from="~/input/twist" to="$(arg input_topic)" />
+  </node>
+
+</launch>
+

mas_common_robotics/mcr_common/mcr_common_converters/ros/src/mbot_looking_while_walking.cpp

@@ -0,0 +1,139 @@
+/*
+ * Copyright [2015] <Bonn-Rhein-Sieg University>
+ * twist_to_motion_direction_conversion_node.cpp
+ *
+ *  Created on: Mar 28, 2015
+ *      Author: fred
+ */
+
+#include <mcr_common_converters/twist_to_motion_direction_conversion_node.h>
+
+
+#include <string>
+
+#include <math.h>
+
+TwistToMotionDirectionConversionNode::TwistToMotionDirectionConversionNode() :
+    twist_msg_received_(false), event_msg_received_(false), current_state_(INIT)
+{
+    ros::NodeHandle nh("~");
+
+    sub_event_ = nh.subscribe("event_in", 1, &TwistToMotionDirectionConversionNode::eventCallback, this);
+    sub_twist_ = nh.subscribe("/cmd_vel", 1, &TwistToMotionDirectionConversionNode::twistCallback, this);
+    sub_head_position_ = nh.subscribe("/mbot_driver/interaction/mbot_head_angle_joint_state", 1, &TwistToMotionDirectionConversionNode::headCallback, this);
+    //pub_pose_ = nh.advertise < geometry_msgs::PoseStamped > ("output/pose", 1);
+    //pub_point_ = nh.advertise < geometry_msgs::PointStamped > ("output/point", 1);
+    pub_head_ = nh.advertise < std_msgs::UInt8MultiArray > ("/cmd_head", 1);
+
+
+    nh.param < std::string > ("frame_id", frame_id_, "/base_link");
+    nh.param<double>("distance_to_frame", distance_to_frame_, 1.5);
+}
+
+TwistToMotionDirectionConversionNode::~TwistToMotionDirectionConversionNode()
+{
+    sub_event_.shutdown();
+    sub_twist_.shutdown();
+    sub_head_position_.shutdown();
+    //pub_pose_.shutdown();
+    //pub_point_.shutdown();
+    pub_head_.shutdown();
+}
+
+void TwistToMotionDirectionConversionNode::twistCallback(const geometry_msgs::TwistPtr &msg)
+{
+    twist_msg_ = *msg;
+    twist_msg_received_ = true;
+}
+
+void TwistToMotionDirectionConversionNode::eventCallback(const std_msgs::StringPtr &msg)
+{
+    event_msg_ = *msg;
+    event_msg_received_ = true;
+}
+
+void TwistToMotionDirectionConversionNode::headCallback(const sensor_msgs::JointStatePtr &msg)
+{
+    head_msg_ = *msg;
+}
+
+void TwistToMotionDirectionConversionNode::computeMotionDirectionAndPublish()
+{
+    // if there is not motion, return
+    if (fabs(twist_msg_.linear.x) + fabs(twist_msg_.linear.y) + fabs(twist_msg_.angular.z) == 0)
+        return;
+
+    double motion_direction = 0.00;
+
+    int min_angle = 5;      // degrees
+    int max_angle = 175;
+    float last_direction;
+
+    std_msgs::UInt8MultiArray controlMsg;
+    controlMsg.data.resize(2);
+
+    std::string pos_msg;
+    std::string speed_msg;
+
+    // motion_direction based on base_twist_msg
+    // zero degrees is looking front (North)
+    motion_direction = 90 - (getMotionDirectionFromTwist2D(twist_msg_.linear.x, twist_msg_.linear.y, twist_msg_.angular.z)* 180 / M_PI);
+
+    controlMsg.data[1] = 50; // head rotating speed
+
+    if (motion_direction > min_angle && motion_direction < max_angle)
+    {
+		// only turns head if angle is admissible
+        controlMsg.data[0] = int(motion_direction);
+        //ROS_INFO_STREAM("Turning head to"<< controlMsg.data[0] << " with "<< controlMsg.data[1] << " speed");
+		pub_head_.publish(controlMsg);
+    }
+}
+
+void TwistToMotionDirectionConversionNode::update()
+{
+    // check if a new event has been received
+    if (event_msg_received_)
+    {
+        ROS_INFO_STREAM("Received event: " << event_msg_.data);
+
+        if (event_msg_.data == "e_start")
+            current_state_ = RUN;
+        else if (event_msg_.data == "e_stop")
+            current_state_ = INIT;
+        else
+            ROS_ERROR_STREAM("Event not supported: " << event_msg_.data);
+
+
+        event_msg_received_ = false;
+    }
+
+    // if state is INIT, do nothing
+    if (current_state_ == INIT)
+        return;
+
+    // if not msg received, do nothing
+    if (!twist_msg_received_)
+        return;
+
+    computeMotionDirectionAndPublish();
+
+    twist_msg_received_ = false;
+}
+
+int main(int argc, char** argv)
+{    ros::init(argc, argv, "twist_to_motion_direction_conversion");
+    ros::NodeHandle nh("~");
+
+    TwistToMotionDirectionConversionNode conversion_node = TwistToMotionDirectionConversionNode();
+
+    ros::Rate loop_rate(30);
+
+    while (ros::ok())
+    {
+        conversion_node.update();
+        ros::spinOnce();
+        loop_rate.sleep();
+    }
+    return 0;
+}