stable

Author: zhao1995

drv_brain/launch/includes/drv_workstation_v2.launch.xml

@@ -12,18 +12,18 @@
 
   <!-- broadcast transform among frames -->
   <node ns="$(arg vision_namespace)" name="drv_tf" pkg="drv_tf" type="drv_tf" output="screen">
-    <param name="camera_frame_id" value="/$(arg camera_frame)" />
-    <param name="root_frame_id" value="/$(arg base_link_frame)" />
+    <param name="camera_frame_id" value="$(arg camera_frame)" />
+    <param name="root_frame_id" value="$(arg base_link_frame)" />
 
     <!-- Modify following values according to the structure of NVG 2.0 -->
     <param name="pitch_offset_value" value="90.0" />
 
     <param name="dx_camera_to_pitch_value" value="0.0" />
-    <param name="dy_camera_to_pitch_value" value="0.03" />
+    <param name="dy_camera_to_pitch_value" value="0.06" />
     <param name="dz_camera_to_pitch_value" value="0.0" />
-    <param name="dx_pitch_to_yaw_value" value="0.01" />
-    <param name="dy_pitch_to_yaw_value" value="0.01" />
-    <param name="dz_pitch_to_yaw_value" value="0.01" />
+    <param name="dx_pitch_to_yaw_value" value="0.12" />
+    <param name="dy_pitch_to_yaw_value" value="0.0" />
+    <param name="dz_pitch_to_yaw_value" value="0.159" />
   </node>
 
 
@@ -32,15 +32,15 @@
   </node>
 
   <!-- initialize user selection service -->
-  <node name="drv_user" pkg="drv_user" type="drv_user.py" output="screen">
+  <node ns="$(arg vision_namespace)" name="drv_user" pkg="drv_user" type="drv_user.py" output="screen">
   </node>
 
   <!-- initialize search client -->
   <node ns="$(arg vision_namespace)" name="drv_search" pkg="drv_search" type="drv_search" output="screen">
   </node>
 
   <!-- initialize tracking function -->
-  <node ns="$(arg vision_namespace)" name="drv_track_node" pkg="drv_track" type="drv_track_node" output="screen">
+  <node ns="$(arg vision_namespace)" name="drv_track_node" pkg="drv_track" type="drv_track_node">
   </node>
 
   <!-- initialize grasp planning function -->
@@ -51,7 +51,7 @@
   </node>
 
   <!-- initialize face recognition service -->
-  <node name="drv_face_service" pkg="drv_face_service" type="drv_face_service" output="screen">
+  <node ns="$(arg vision_namespace)" name="drv_face_service" pkg="drv_face_service" type="drv_face_service" output="screen">
     <param name="face_likehood_threshold" value="0.9"/>
   </node>
 
@@ -61,7 +61,7 @@
 
   <!-- initialize visualization -->
   <param name="use_gui" value="true"/> 
-  <node name="rviz" pkg="rviz" type="rviz" >
+  <node ns="$(arg vision_namespace)" name="rviz" pkg="rviz" type="rviz" >
   </node>
 
 

drv_brain/src/drv_brain.cpp

@@ -91,11 +91,11 @@ void pubInfo(string info)
 
 void teleOpCallback(const std_msgs::Int32MultiArrayConstPtr &msg)
 {
-  if (msg->data.empty())
+  if (msg->data.empty() || (msg->data[0] == 0 && msg->data[1] ==0))
     return;
 
   int pitch_temp = pitchAngle_ + msg->data[0];
-  if (pitch_temp < 40 || pitch_temp > 130)
+  if (pitch_temp < 60 || pitch_temp > 140)
     return;
   else
     pitchAngle_ = pitch_temp;
@@ -153,14 +153,14 @@ void trackCallback(const std_msgs::BoolConstPtr &msg)
   {
     if (!msg->data)
     {
-      ROS_INFO_THROTTLE(21, "Target lost!");
+      ROS_INFO_THROTTLE(21, "Track report: Target lost!");
       foundTarget_ = false;
       ros::param::set(param_vision_feedback_track, -1);
       ros::param::set(param_vision_feedback, 1);
     }
     else
     {
-      ROS_INFO_THROTTLE(21, "Tracking the target...");
+      ROS_INFO_THROTTLE(21, "Track report: Tracking the target...");
       foundTarget_ = true;
       ros::param::set(param_vision_feedback_track, 1);
       ros::param::set(param_vision_feedback, 1);
@@ -174,13 +174,13 @@ void graspCallback(const std_msgs::BoolConstPtr &msg)
   {
     if (!msg->data)
     {
-      ROS_INFO_THROTTLE(21, "Failed to locate the target.");
+      ROS_INFO_THROTTLE(21, "Grasp report: Failed to locate the target.");
       ros::param::set(param_vision_feedback_grasp, -1);
       ros::param::set(param_vision_feedback, 1);
     }
     else
     {
-      ROS_INFO_THROTTLE(21, "Target location confirmed.");
+      ROS_INFO_THROTTLE(21, "Grasp report: Target location confirmed.");
       ros::param::set(param_vision_feedback_grasp, 1);
       ros::param::set(param_vision_feedback, 3);
     }
@@ -278,7 +278,7 @@ int main(int argc, char **argv)
     // Initialize servo position
     if (!servo_initialized_)
     {
-      pubServo(70, 90);
+      pubServo(90, 90);
       servo_initialized_ = true;
       ROS_INFO("Servo initialized.\n");
     }

drv_face_client/src/drv_face_client.cpp

@@ -125,7 +125,7 @@ int main(int argc, char **argv)
   image_transport::ImageTransport rgb_it(nh);
   facePubImage_ = rgb_it.advertise("search/labeled_image", 1);
 
-  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("rgb/image_rect_color", 1, imageCallback, hints_rgb);
+  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("image_rect_color", 1, imageCallback, hints_rgb);
 
   ros::ServiceClient client = nh.serviceClient<drv_msgs::face_recognize>("drv_face_service");
 

drv_face_service/src/drv_face_service.cpp

@@ -7,56 +7,59 @@
 
 #include "processface.h"
 
-char* caffe_path_env = std::getenv("Caffe_ROOT");
+char* drv_path_env = std::getenv("DRV");
 
-std::string caffe_path = std::string(caffe_path_env);
+std::string drv_path = std::string(drv_path_env);
 
-std::string prototxt = caffe_path + "/models/neu_face/neu_face_deploy.prototxt";
-std::string caffemodel = caffe_path + "/models/neu_face/finetune_neu_face.caffemodel";
+std::string prototxt = drv_path + "/supplements/face_recognize/neu_face_deploy.prototxt";
+std::string caffemodel = drv_path + "/supplements/face_recognize/finetune_neu_face.caffemodel";
 ProcessFace pf_(prototxt, caffemodel, 0, false);
 
 bool recognize_face(drv_msgs::face_recognize::Request  &req,
                     drv_msgs::face_recognize::Response &res)
 {
-    size_t im = req.images_in.size();
-    vector<int> result_id;
-    for (size_t i = 0;i < im; i++)
-        {
-            cv_bridge::CvImagePtr src = cv_bridge::toCvCopy(req.images_in[i], "bgr8");
-            int id = 0;
-            float result_trust = 0.0;
-            pf_.processFace(src->image, id, result_trust);
-
-            if (result_trust < 0.9)
-                result_id.push_back(0);
-            else
-                result_id.push_back(id + 1); // result id start at 0, while known person id start at 1
-        }
-    res.face_label_ids = result_id;
-    return true; // return bool is necessary for service
+  size_t im = req.images_in.size();
+  vector<int> result_id;
+  for (size_t i = 0;i < im; i++)
+  {
+    cv_bridge::CvImagePtr src = cv_bridge::toCvCopy(req.images_in[i], "bgr8");
+    int id = 0;
+    float result_trust = 0.0;
+    pf_.processFace(src->image, id, result_trust);
+    
+    if (result_trust < 0.9)
+      result_id.push_back(0);
+    else
+      result_id.push_back(id + 1); // result id start at 0, while known person id start at 1
+  }
+  res.face_label_ids = result_id;
+  return true; // return bool is necessary for service
 }
 
 int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "face_recognize_server");
-    ros::NodeHandle n;
-
-    std::ifstream f_pt(prototxt.c_str());
-    if (!f_pt)
-    {
-       ROS_ERROR("Can not find prototxt for face detection.");
-       return -1;
-    }
-    std::ifstream f_mo(caffemodel.c_str());
-    if (!f_mo)
-    {
-       ROS_ERROR("Can not find caffemodel for face detection.");
-       return -1;
-    }
-
-    ros::ServiceServer service = n.advertiseService("drv_face_service", recognize_face);
-    ROS_INFO("Ready to recognize face.");
-    ros::spin();
-
-    return 0;
+  ros::init(argc, argv, "face_recognize_server");
+  ros::NodeHandle n;
+  ros::NodeHandle pnh("~");
+  
+  
+  
+  std::ifstream f_pt(prototxt.c_str());
+  if (!f_pt)
+  {
+    ROS_ERROR("Can not find prototxt for face detection.");
+    return -1;
+  }
+  std::ifstream f_mo(caffemodel.c_str());
+  if (!f_mo)
+  {
+    ROS_ERROR("Can not find caffemodel for face detection.");
+    return -1;
+  }
+  
+  ros::ServiceServer service = n.advertiseService("drv_face_service", recognize_face);
+  ROS_INFO("Ready to recognize face.");
+  ros::spin();
+  
+  return 0;
 }

drv_grasp/src/drv_grasp.cpp

@@ -349,7 +349,7 @@ int main(int argc, char **argv)
   ros::init(argc, argv, "drv_grasp");
 
   ros::NodeHandle nh;
-  ros::NodeHandle pnh;
+  ros::NodeHandle pnh("~");
 
   pnh.getParam("root_frame_id", root_frame_);
   pnh.getParam("camera_optical_frame_id", camera_optical_frame_);

drv_motor/src/drv_motor.cpp

@@ -12,7 +12,6 @@
 #include <std_msgs/Int8.h>
 #include <std_msgs/UInt16MultiArray.h>
 #include <std_msgs/Float32MultiArray.h>
-#include <std_msgs/Int32MultiArray.h>
 
 
 #include <stdio.h>
@@ -44,16 +43,18 @@ float calculate(float ab, float bc, float cd, float ad, float theta_a)
   return to_deg(result);
 }
 
-void calculatePitch(float &pitch_angle)
+void setBoundary(int &pitch_angle, int &yaw_angle)
 {
   // Set the boundry of pitch value for NVG 2.0
   if (pitch_angle > 140) pitch_angle = 140.0;
-  if (pitch_angle < 60) pitch_angle = 60.0;
+  else if (pitch_angle < 60) pitch_angle = 60.0;
+  if (yaw_angle > 180) yaw_angle = 180.0;
+  else if (yaw_angle < 0) yaw_angle = 0.0;
 }
 
-void motorPublish(float pitch_angle, float yaw_angle, float pitch_speed, float yaw_speed)
+void motorPublish(int pitch_angle, int yaw_angle, int pitch_speed, int yaw_speed)
 {
-  std_msgs::Float32MultiArray array;
+  std_msgs::UInt16MultiArray array;
   array.data.push_back(pitch_angle);
   array.data.push_back(yaw_angle);
   array.data.push_back(pitch_speed);
@@ -64,10 +65,10 @@ void motorPublish(float pitch_angle, float yaw_angle, float pitch_speed, float y
 void servoCallback(const std_msgs::UInt16MultiArrayConstPtr &msg)
 {
   // this callback should always active
-  float pitchAngle = 70.0;
-  float yawAngle = 90.0;
-  float pitchSpeed = 50.0;
-  float yawSpeed = 50.0;
+  int pitchAngle = 90;
+  int yawAngle = 90;
+  int pitchSpeed = 50;
+  int yawSpeed = 50;
   if (msg->data.size() == 2) {
     pitchAngle = msg->data[0];
     yawAngle = msg->data[1];
@@ -88,7 +89,7 @@ void servoCallback(const std_msgs::UInt16MultiArrayConstPtr &msg)
     ROS_WARN_THROTTLE(11, "Servo message has wrong number of params.");
   }
 
-  calculatePitch(pitchAngle);
+  setBoundary(pitchAngle, yawAngle);
   motorPublish(pitchAngle, yawAngle, pitchSpeed, yawSpeed);
 }
 
@@ -98,7 +99,7 @@ int main(int argc, char **argv)
 
   ros::NodeHandle nh;
 
-  motorPub_ = nh.advertise<std_msgs::Float32MultiArray>("motor", 1, true);
+  motorPub_ = nh.advertise<std_msgs::UInt16MultiArray>("motor", 1, true);
 
   // This node should be launched in namespace /vision
   ros::Subscriber sub_servo_cmd = nh.subscribe<std_msgs::UInt16MultiArray>("servo", 2, servoCallback);

drv_recognize/scripts/drv_recognize_client.py

@@ -3,7 +3,7 @@
 import rospy
 import cv2
 from drv_msgs.srv import *
-from sensor_msgs.msg import Image
+from sensor_msgs.msg import CompressedImage
 from cv_bridge import CvBridge
 
 
@@ -25,16 +25,14 @@ def image_callback(img_msg):
 
     bg = CvBridge()
     cv_img_labeled = bg.imgmsg_to_cv2(resp.img_out, 'bgr8')
-    cv2.imshow("Image window", cv_img_labeled)
-    cv2.waitKey(1)
 
     print resp.obj_info.labels
     print resp.obj_info.box_array
 
 
 def image_listener():
     rospy.init_node('recognize_client', anonymous=True)
-    rospy.Subscriber('rgb/image_rect_color', Image, image_callback, queue_size=1, buff_size=921600)
+    rospy.Subscriber('image_rect_color', CompressedImage, image_callback, queue_size=1, buff_size=921600)
 
     while not rospy.is_shutdown():
         rospy.spin()

drv_search/src/drv_search.cpp

@@ -124,7 +124,7 @@ int main(int argc, char **argv)
   searchPubTarget_ = nh.advertise<drv_msgs::recognized_target>("search/recognized_target", 1, true);
 
   //		ros::Subscriber sub_rgb = nh.subscribe("/rgb/image/compressed", 1, imageCallback);
-  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("rgb/image_rect_color", 1, imageCallback, hints_rgb);
+  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("image_rect_color", 1, imageCallback, hints_rgb);
   //		ros::Subscriber sub_depth = nh.subscribe("/depth/image_raw", 1, depthCallback );
 
   ros::ServiceClient client = nh.serviceClient<drv_msgs::recognize>("drv_recognize");

drv_tf/src/drv_tf.cpp

@@ -54,7 +54,7 @@ std::string cameraPitchFrame_ = "/camera_pitch_frame";
 std::string cameraYawFrame_ = "/camera_yaw_frame"; // Base of NEU Vision Gear
 std::string rootFrame_ = "/base_link"; // Root frame that NVG link to
 
-MoveMean mm(50); // the value represent the strengh to stablize the camera.
+MoveMean mm(10); // the value represent the strengh to stablize the camera.
 
 void configCallback(drv_tf::tfConfig &config, uint32_t level)
 {
@@ -72,16 +72,15 @@ void servoCallback(const std_msgs::UInt16MultiArrayConstPtr &msg)
 
 void imuPitchCallback(const std_msgs::Float32ConstPtr & msg)
 {
-  float imuPitchTemp = 0.0;
   pitch_ = msg->data - pitch_offset_; // notice that pitch_ < 0
-  mm.getMoveMean(pitch_, imuPitchTemp);
-  pitch_ = imuPitchTemp * to_rad;
+  mm.getMoveMean(pitch_);
+  pitch_ *= to_rad;
 }
 
 void imuYawCallback(const std_msgs::Float32ConstPtr & msg)
 {
   // This callback only valid for NVG 2.0
-  yaw_ = (msg->data - 90) * to_rad;
+//  yaw_ = (msg->data - 90) * to_rad;
 }
 
 void imageCallback(const sensor_msgs::ImageConstPtr &image_msg)
@@ -177,7 +176,7 @@ int main(int argc, char** argv){
   ros::NodeHandle rgb_pnh(pnh, "rgb");
   image_transport::ImageTransport it_rgb_sub(rgb_nh);
   image_transport::TransportHints hints_rgb("compressed", ros::TransportHints(), rgb_pnh);
-  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("rgb/image_rect_color", 1, imageCallback, hints_rgb);
+  image_transport::Subscriber sub_rgb = it_rgb_sub.subscribe("image_rect_color", 1, imageCallback, hints_rgb);
 
   ROS_INFO("TF initialized.");