Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
robotis_op::HeadControlModule Class Reference

#include <head_control_module.h>

Inheritance diagram for robotis_op::HeadControlModule:
Inheritance graph
[legend]

Public Member Functions

 HeadControlModule ()
 
void initialize (const int control_cycle_msec, robotis_framework::Robot *robot)
 
bool isRunning ()
 
void onModuleDisable ()
 
void onModuleEnable ()
 
void process (std::map< std::string, robotis_framework::Dynamixel * > dxls, std::map< std::string, double > sensors)
 
void stop ()
 
virtual ~HeadControlModule ()
 
- Public Member Functions inherited from robotis_framework::MotionModule
ControlMode getControlMode ()
 
bool getModuleEnable ()
 
std::string getModuleName ()
 
void setModuleEnable (bool enable)
 
virtual ~MotionModule ()
 

Private Types

enum  {
  NoScan = 0, TopLeft = 1, BottomRight = 2, BottomLeft = 3,
  TopRight = 4
}
 

Private Member Functions

Eigen::MatrixXd calcMinimumJerkTraPVA (double pos_start, double vel_start, double accel_start, double pos_end, double vel_end, double accel_end, double smp_time, double mov_time)
 
bool checkAngleLimit (const int joint_index, double &goal_position)
 
void finishMoving ()
 
void generateScanTra (const int head_direction)
 
void jointTraGeneThread ()
 
void publishStatusMsg (unsigned int type, std::string msg)
 
void queueThread ()
 
void setHeadJoint (const sensor_msgs::JointState::ConstPtr &msg, bool is_offset)
 
void setHeadJointCallback (const sensor_msgs::JointState::ConstPtr &msg)
 
void setHeadJointOffsetCallback (const sensor_msgs::JointState::ConstPtr &msg)
 
void setHeadScanCallback (const std_msgs::String::ConstPtr &msg)
 
void startMoving ()
 
void stopMoving ()
 

Private Attributes

double angle_unit_
 
Eigen::MatrixXd calc_joint_accel_tra_
 
Eigen::MatrixXd calc_joint_tra_
 
Eigen::MatrixXd calc_joint_vel_tra_
 
int control_cycle_msec_
 
Eigen::MatrixXd current_position_
 
const bool DEBUG
 
Eigen::MatrixXd goal_acceleration_
 
Eigen::MatrixXd goal_position_
 
Eigen::MatrixXd goal_velocity_
 
bool has_goal_position_
 
bool is_direct_control_
 
bool is_moving_
 
std::string last_msg_
 
ros::Time last_msg_time_
 
std::map< int, double > max_angle_
 
std::map< int, double > min_angle_
 
double moving_time_
 
boost::thread queue_thread_
 
int scan_state_
 
ros::Publisher status_msg_pub_
 
bool stop_process_
 
Eigen::MatrixXd target_position_
 
int tra_count_
 
boost::thread * tra_gene_thread_
 
boost::mutex tra_lock_
 
int tra_size_
 
std::map< std::string, int > using_joint_name_
 

Additional Inherited Members

- Static Public Member Functions inherited from robotis_framework::Singleton< HeadControlModule >
static void destroyInstance ()
 
static T * getInstance ()
 
- Public Attributes inherited from robotis_framework::MotionModule
std::map< std::string, DynamixelState * > result_
 
- Protected Member Functions inherited from robotis_framework::Singleton< HeadControlModule >
Singletonoperator= (Singleton const &)
 
 Singleton (Singleton const &)
 
 Singleton ()
 
- Protected Attributes inherited from robotis_framework::MotionModule
ControlMode control_mode_
 
bool enable_
 
std::string module_name_
 

Detailed Description

Definition at line 40 of file head_control_module.h.

Member Enumeration Documentation

anonymous enum
private
Enumerator
NoScan 
TopLeft 
BottomRight 
BottomLeft 
TopRight 

Definition at line 56 of file head_control_module.h.

Constructor & Destructor Documentation

robotis_op::HeadControlModule::HeadControlModule ( )

Definition at line 25 of file head_control_module.cpp.

robotis_op::HeadControlModule::~HeadControlModule ( )
virtual

Definition at line 62 of file head_control_module.cpp.

Member Function Documentation

Eigen::MatrixXd robotis_op::HeadControlModule::calcMinimumJerkTraPVA ( double  pos_start,
double  vel_start,
double  accel_start,
double  pos_end,
double  vel_end,
double  accel_end,
double  smp_time,
double  mov_time 
)
private

Definition at line 485 of file head_control_module.cpp.

bool robotis_op::HeadControlModule::checkAngleLimit ( const int  joint_index,
double &  goal_position 
)
private

Definition at line 209 of file head_control_module.cpp.

void robotis_op::HeadControlModule::finishMoving ( )
private

Definition at line 351 of file head_control_module.cpp.

void robotis_op::HeadControlModule::generateScanTra ( const int  head_direction)
private

Definition at line 414 of file head_control_module.cpp.

void robotis_op::HeadControlModule::initialize ( const int  control_cycle_msec,
robotis_framework::Robot robot 
)
virtual

Implements robotis_framework::MotionModule.

Definition at line 67 of file head_control_module.cpp.

bool robotis_op::HeadControlModule::isRunning ( )
virtual

Implements robotis_framework::MotionModule.

Definition at line 316 of file head_control_module.cpp.

void robotis_op::HeadControlModule::jointTraGeneThread ( )
private

Definition at line 532 of file head_control_module.cpp.

void robotis_op::HeadControlModule::onModuleDisable ( )
virtual

Reimplemented from robotis_framework::MotionModule.

Definition at line 326 of file head_control_module.cpp.

void robotis_op::HeadControlModule::onModuleEnable ( )
virtual

Reimplemented from robotis_framework::MotionModule.

Definition at line 321 of file head_control_module.cpp.

void robotis_op::HeadControlModule::process ( std::map< std::string, robotis_framework::Dynamixel * >  dxls,
std::map< std::string, double >  sensors 
)
virtual

Implements robotis_framework::MotionModule.

Definition at line 229 of file head_control_module.cpp.

void robotis_op::HeadControlModule::publishStatusMsg ( unsigned int  type,
std::string  msg 
)
private

Definition at line 582 of file head_control_module.cpp.

void robotis_op::HeadControlModule::queueThread ( )
private

Definition at line 85 of file head_control_module.cpp.

void robotis_op::HeadControlModule::setHeadJoint ( const sensor_msgs::JointState::ConstPtr &  msg,
bool  is_offset 
)
private

Definition at line 115 of file head_control_module.cpp.

void robotis_op::HeadControlModule::setHeadJointCallback ( const sensor_msgs::JointState::ConstPtr &  msg)
private

Definition at line 105 of file head_control_module.cpp.

void robotis_op::HeadControlModule::setHeadJointOffsetCallback ( const sensor_msgs::JointState::ConstPtr &  msg)
private

Definition at line 110 of file head_control_module.cpp.

void robotis_op::HeadControlModule::setHeadScanCallback ( const std_msgs::String::ConstPtr &  msg)
private

Definition at line 184 of file head_control_module.cpp.

void robotis_op::HeadControlModule::startMoving ( )
private

Definition at line 344 of file head_control_module.cpp.

void robotis_op::HeadControlModule::stop ( )
virtual

Implements robotis_framework::MotionModule.

Definition at line 304 of file head_control_module.cpp.

void robotis_op::HeadControlModule::stopMoving ( )
private

Definition at line 396 of file head_control_module.cpp.

Member Data Documentation

double robotis_op::HeadControlModule::angle_unit_
private

Definition at line 98 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::calc_joint_accel_tra_
private

Definition at line 107 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::calc_joint_tra_
private

Definition at line 105 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::calc_joint_vel_tra_
private

Definition at line 106 of file head_control_module.h.

int robotis_op::HeadControlModule::control_cycle_msec_
private

Definition at line 85 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::current_position_
private

Definition at line 101 of file head_control_module.h.

const bool robotis_op::HeadControlModule::DEBUG
private

Definition at line 90 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::goal_acceleration_
private

Definition at line 104 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::goal_position_
private

Definition at line 102 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::goal_velocity_
private

Definition at line 103 of file head_control_module.h.

bool robotis_op::HeadControlModule::has_goal_position_
private

Definition at line 97 of file head_control_module.h.

bool robotis_op::HeadControlModule::is_direct_control_
private

Definition at line 93 of file head_control_module.h.

bool robotis_op::HeadControlModule::is_moving_
private

Definition at line 92 of file head_control_module.h.

std::string robotis_op::HeadControlModule::last_msg_
private

Definition at line 114 of file head_control_module.h.

ros::Time robotis_op::HeadControlModule::last_msg_time_
private

Definition at line 113 of file head_control_module.h.

std::map<int, double> robotis_op::HeadControlModule::max_angle_
private

Definition at line 110 of file head_control_module.h.

std::map<int, double> robotis_op::HeadControlModule::min_angle_
private

Definition at line 111 of file head_control_module.h.

double robotis_op::HeadControlModule::moving_time_
private

Definition at line 95 of file head_control_module.h.

boost::thread robotis_op::HeadControlModule::queue_thread_
private

Definition at line 86 of file head_control_module.h.

int robotis_op::HeadControlModule::scan_state_
private

Definition at line 96 of file head_control_module.h.

ros::Publisher robotis_op::HeadControlModule::status_msg_pub_
private

Definition at line 89 of file head_control_module.h.

bool robotis_op::HeadControlModule::stop_process_
private

Definition at line 91 of file head_control_module.h.

Eigen::MatrixXd robotis_op::HeadControlModule::target_position_
private

Definition at line 100 of file head_control_module.h.

int robotis_op::HeadControlModule::tra_count_
private

Definition at line 94 of file head_control_module.h.

boost::thread* robotis_op::HeadControlModule::tra_gene_thread_
private

Definition at line 87 of file head_control_module.h.

boost::mutex robotis_op::HeadControlModule::tra_lock_
private

Definition at line 88 of file head_control_module.h.

int robotis_op::HeadControlModule::tra_size_
private

Definition at line 94 of file head_control_module.h.

std::map<std::string, int> robotis_op::HeadControlModule::using_joint_name_
private

Definition at line 109 of file head_control_module.h.


The documentation for this class was generated from the following files:


op3_head_control_module
Author(s): Kayman
autogenerated on Mon Jun 10 2019 14:41:10