real_arm.cpp

Go to the documentation of this file.

#include "sr_hand/hand/real_arm.h"
// our robot code
#include <robot/robot.h>
#include <robot/hand.h>
#include <robot/hand_protocol.h>

#include <string>
#include <vector>
#include <time.h>
#include <ros/ros.h>

namespace shadowrobot
{
  RealArm::RealArm() :
          SRArticulatedRobot()
  {
    /* We need to attach the program to the robot, or fail if we cannot. */
    if (robot_init() < 0)
    {
      ROS_FATAL("Robot interface broken\n");
      ROS_BREAK();
    }

    /* We need to attach the program to the hand as well, or fail if we cannot. */
    if (hand_init() < 0)
    {
      ROS_FATAL("Arm interface broken\n");
      ROS_BREAK();
    }

    srand(time(NULL));
    initializeMap();
  }

  RealArm::~RealArm()
  {
  }

  void RealArm::initializeMap()
  {
    joints_map_mutex.lock();
    JointData tmpData;

    tmpData.position = 0.0;
    tmpData.target = 0.0;
    tmpData.temperature = 0.0;
    tmpData.current = 0.0;
    tmpData.force = 0.0;
    tmpData.flags = "";

    for (unsigned int i = START_OF_ARM; i < NUM_HAND_JOINTS; ++i)
    {
      std::string name = hand_joints[i].joint_name;
      tmpData.jointIndex = i;

      joints_map[name] = tmpData;

      ROS_INFO("NAME[%d]: %s ", i, name.c_str());
    }

    joints_map_mutex.unlock();
  }

  int16_t RealArm::sendupdate(std::string joint_name, double target)
  {
    joints_map_mutex.lock();

    // search the sensor in the hash_map
    JointsMap::iterator iter = joints_map.find(joint_name);

    if (iter != joints_map.end())
    {
      JointData tmpData = joints_map.find(joint_name)->second;
      int index_arm_joints = tmpData.jointIndex;

      // trim to the correct range
      if (target < hand_joints[index_arm_joints].min_angle)
      {
        target = hand_joints[index_arm_joints].min_angle;
      }
      if (target > hand_joints[index_arm_joints].max_angle)
      {
        target = hand_joints[index_arm_joints].max_angle;
      }

      // here we update the actual hand angles
      robot_update_sensor(&hand_joints[index_arm_joints].joint_target, target);
      joints_map_mutex.unlock();
      return 0;
    }

    ROS_DEBUG("Joint %s not found", joint_name.c_str());

    joints_map_mutex.unlock();
    return -1;
  }

  JointData RealArm::getJointData(std::string joint_name)
  {
    joints_map_mutex.lock();
    JointsMap::iterator iter = joints_map.find(joint_name);

    // joint not found
    if (iter == joints_map.end())
    {
      ROS_ERROR("Joint %s not found.", joint_name.c_str());
      JointData noData;
      noData.position = 0.0;
      noData.target = 0.0;
      noData.temperature = 0.0;
      noData.current = 0.0;
      noData.force = 0.0;
      noData.flags = "";
      noData.jointIndex = 0;

      joints_map_mutex.unlock();
      return noData;
    }

    // joint found
    JointData tmpData = iter->second;
    int index = tmpData.jointIndex;

    tmpData.position = robot_read_sensor(&hand_joints[index].position);
    tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);

    joints_map[joint_name] = tmpData;

    joints_map_mutex.unlock();
    return tmpData;
  }

  SRArticulatedRobot::JointsMap RealArm::getAllJointsData()
  {
    // update the map for each joints
    for (JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
    {
      getJointData(it->first);
    }

    JointsMap tmp = JointsMap(joints_map);

    // return the map
    return tmp;
  }

  int16_t RealArm::setContrl(std::string contrlr_name, JointControllerData ctrlr_data)
  {
    ROS_WARN("The setContrl method is not yet implemented");
    return 0;
  }

  JointControllerData RealArm::getContrl(std::string contrlr_name)
  {
    ROS_WARN("The getContrl method is not yet implemented");
    JointControllerData no_result;
    return no_result;
  }

  int16_t RealArm::setConfig(std::vector <std::string> myConfig)
  {
    ROS_WARN("The set config function is not implemented in the virtual arm.");
    return 0;
  }

  void RealArm::getConfig(std::string joint_name)
  {
    ROS_WARN("The get config function is not implemented in the virtual arm.");
  }

  std::vector <DiagnosticData> RealArm::getDiagnostics()
  {
    joints_map_mutex.lock();
    std::vector <DiagnosticData> returnVect;

    // @todo: read diagnostic data from the robot

    for (JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
    {
      DiagnosticData tmpDiag;
      tmpDiag.joint_name = it->first;
      tmpDiag.level = 0;
      tmpDiag.flags = "";
      tmpDiag.target_sensor_num = 0;
      tmpDiag.target = it->second.target;
      tmpDiag.position_sensor_num = 0;
      tmpDiag.position = it->second.position;

      returnVect.push_back(tmpDiag);
    }

    joints_map_mutex.unlock();
    return returnVect;
  }

}  // namespace shadowrobot