Skip to content

Commit

Permalink
things are compiling and seemingly "complete". not yet tested though
Browse files Browse the repository at this point in the history
  • Loading branch information
Stephen Hart committed Feb 23, 2015
1 parent e59acaf commit 7157988
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 24 deletions.
1 change: 0 additions & 1 deletion pronto_translator/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ find_package(catkin REQUIRED COMPONENTS
sensor_msgs
nav_msgs
pronto_translator_msgs
atlas_hardware_interface
)

## System dependencies are found with CMake's conventions
Expand Down
19 changes: 9 additions & 10 deletions pronto_translator/nodes/pronto_helper.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@

from std_msgs.msg import Int32
from sensor_msgs.msg import JointState, Imu
from geometry_msgs.msg import Pose, PoseStamped
from geometry_msgs.msg import Pose, PoseStamped, Twist, TwistWithCovariance
from nav_msgs.msg import Odometry
from atlas_hardware_interface.msg import AtlasControlDataFromRobot, AtlasOdometry

Expand All @@ -21,15 +21,14 @@ class ProntoHelper:

def __init__(self):

self.atlas_data_sub = rospy.Subscriber('/atlas_hardware/data/full', AtlasControlDataFromRobot, self.atlas_data_cb)
self.atlas_odom_sub = rospy.Subscriber('/atlas_hardware/data/odometry', AtlasOdometry, self.atlas_odom_cb)

self.imu_pub = rospy.Publisher('/pronto_helper/imu', Imu, queue_size=10)
self.raw_imu_pub = rospy.Publisher('/pronto_helper/raw_imu', CachedRawIMUData, queue_size=10)
self.behavior_pub = rospy.Publisher('/pronto_helper/behavior', Int32, queue_size=10)
self.footsensor_pub = rospy.Publisher('/pronto_helper/foot_sensor', FootSensor, queue_size=10)
self.odometry_pub = rospy.Publisher('/pronto_helper/odom', Odometry, queue_size=10)

self.atlas_data_sub = rospy.Subscriber('/atlas_hardware/data/full', AtlasControlDataFromRobot, self.atlas_data_cb)
self.atlas_odom_sub = rospy.Subscriber('/atlas_hardware/data/odometry', AtlasOdometry, self.atlas_odom_cb)

def atlas_data_cb(self, data) :

Expand Down Expand Up @@ -75,17 +74,17 @@ def atlas_odom_cb(self, data) :
odom.header = data.header

odom.child_frame_id = data.header.frame_id
odom.pose.pose.position.x = data.pose_est.position.x
odom.pose.pose.position.y = data.pose_est.position.y
odom.pose.pose.position.z = data.pose_est.position.z
odom.pose.pose.position.x = data.pos_est.position.x
odom.pose.pose.position.y = data.pos_est.position.y
odom.pose.pose.position.z = data.pos_est.position.z
odom.pose.pose.orientation.x = data.filtered_imu.orientation.x
odom.pose.pose.orientation.y = data.filtered_imu.orientation.y
odom.pose.pose.orientation.z = data.filtered_imu.orientation.z
odom.pose.pose.orientation.w = data.filtered_imu.orientation.w
odom.twist = TwistWithCovariance()
odom.twist.twist.linear.x = data.pose_est.velocity.x
odom.twist.twist.linear.y = data.pose_est.velocity.y
odom.twist.twist.linear.z = data.pose_est.velocity.z
odom.twist.twist.linear.x = data.pos_est.velocity.x
odom.twist.twist.linear.y = data.pos_est.velocity.y
odom.twist.twist.linear.z = data.pos_est.velocity.z
odom.twist.twist.angular.x = data.filtered_imu.angular_velocity.x
odom.twist.twist.angular.y = data.filtered_imu.angular_velocity.y
odom.twist.twist.angular.z = data.filtered_imu.angular_velocity.z
Expand Down
78 changes: 65 additions & 13 deletions pronto_translator/src/ros2lcm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ ROS_2_LCM::ROS_2_LCM(ros::NodeHandle node_, bool simulation_) :
lidar_sub_ = node_.subscribe(string("/multisense/lidar_scan"), 100, &ROS_2_LCM::lidar_cb,this); // NEED THIS

// Multisense Joint Angles:
head_joint_states_sub_ = node_.subscribe(string("/spindle_state"), 100, &ROS_2_LCM::head_joint_states_cb,this); // NEED THIS
head_joint_states_sub_ = node_.subscribe(string("/multisense/joint_states"), 100, &ROS_2_LCM::head_joint_states_cb,this); // NEED THIS

};

Expand Down Expand Up @@ -187,8 +187,14 @@ void ROS_2_LCM::lidar_cb(const sensor_msgs::LaserScanConstPtr& msg){

}

int imu_store_counter=0;
void ROS_2_LCM::imu_cb(const sensor_msgs::ImuConstPtr& msg) {

if (imu_store_counter%100 ==0){ // 80
ROS_INFO("IMU STORE [%d]", imu_store_counter );
}
imu_store_counter++;

// copy filtered IMU Data
boost::unique_lock<boost::mutex> scoped_lock(imu_data_mutex_);
imu_data_.header = msg->header;
Expand All @@ -205,7 +211,7 @@ int foot_sensor_counter=0;
void ROS_2_LCM::foot_sensor_cb(const pronto_translator_msgs::FootSensorConstPtr& msg) {

if (foot_sensor_counter%100 ==0){
ROS_INFO("FootSensor[%d]", (int) foot_sensor_counter);
ROS_INFO("FOOT SENSOR[%d]", (int) foot_sensor_counter);
}
foot_sensor_counter++;

Expand Down Expand Up @@ -252,8 +258,14 @@ void ROS_2_LCM::pose_bdi_cb(const nav_msgs::OdometryConstPtr& msg) {

}


int head_joint_counter =0;
void ROS_2_LCM::head_joint_states_cb(const sensor_msgs::JointStateConstPtr& msg){

if (head_joint_counter%100 ==0){
ROS_INFO("HEAD JOINT STATE[%d]", head_joint_counter);
}
head_joint_counter++;

int64_t utime = (int64_t) floor(msg->header.stamp.toNSec()/1000);
float position = msg->position[0];
float velocity = msg->velocity[0];
Expand All @@ -271,13 +283,13 @@ int js_counter=0;
void ROS_2_LCM::joint_states_cb(const sensor_msgs::JointStateConstPtr& msg) {

if (js_counter%500 ==0){
ROS_INFO("Got JointState[%d]", js_counter);
ROS_INFO("JOINT STATE[%d]", js_counter);
}
js_counter++;

std::vector< std::pair<int,int> > jm;

jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shx") , 16 ));
/* jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shx") , 16 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_elx") , 17 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_akx") , 15 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkx") , 2 ));
Expand Down Expand Up @@ -305,8 +317,40 @@ void ROS_2_LCM::joint_states_cb(const sensor_msgs::JointStateConstPtr& msg) {
jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkz") , 0 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_aky") , 8 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_ely") , 27 ));

int n_joints = jm.size();
*/
/*
jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkz") , 1 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "back_bky") , 2 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "back_bkx") , 3 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "neck_ay") , 4 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpz") , 5 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpx") , 6 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_hpy") , 7 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_kny") , 8 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_aky") , 9 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_leg_akx") , 10 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpz") , 11 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpx") , 12 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_hpy") , 13 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_kny") , 14 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_aky") , 15 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_leg_akx") , 16 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_shz") , 17 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_shx") , 18 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_ely") , 19 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_elx") , 20 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_uwy") , 21 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "l_arm_mwx") , 22 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shz") , 23 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_shx") , 24 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_ely") , 25 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_elx") , 26 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_uwy") , 27 ));
jm.push_back ( std::make_pair( getIndex(msg->name, "r_arm_mwx") , 28 ));*/

//int n_joints = jm.size();
int n_joints = msg->name.size();

pronto::atlas_state_t msg_out;
msg_out.utime = (int64_t) msg->header.stamp.toNSec()/1000; // from nsec to usec
Expand All @@ -323,11 +367,14 @@ void ROS_2_LCM::joint_states_cb(const sensor_msgs::JointStateConstPtr& msg) {
msg_out.joint_effort.assign(n_joints , std::numeric_limits<int>::min() );
msg_out.num_joints = n_joints;

for (std::vector<int>::size_type i = 0; i < jm.size(); i++) {
for (std::vector<int>::size_type i = 0; i < n_joints; i++) {
//std::cout << jm[i].first << " and " << jm[i].second << "\n";
msg_out.joint_position[ jm[i].second ] = msg->position[ jm[i].first ];
msg_out.joint_velocity[ jm[i].second ] = msg->velocity[ jm[i].first ];
msg_out.joint_effort[ jm[i].second ] = msg->effort[ jm[i].first ];
//msg_out.joint_position[ jm[i].second ] = msg->position[ jm[i].first ];
//msg_out.joint_velocity[ jm[i].second ] = msg->velocity[ jm[i].first ];
//msg_out.joint_effort[ jm[i].second ] = msg->effort[ jm[i].first ];
msg_out.joint_position[i] = msg->position[i];
msg_out.joint_velocity[i] = msg->velocity[i];
msg_out.joint_effort[i] = msg->effort[i];
}

// App end FT sensor info
Expand All @@ -346,9 +393,14 @@ void ROS_2_LCM::joint_states_cb(const sensor_msgs::JointStateConstPtr& msg) {
}



int raw_imu_counter =0;
void ROS_2_LCM::raw_imu_cb(const pronto_translator_msgs::CachedRawIMUDataConstPtr& msg){

if (raw_imu_counter%100 ==0){
ROS_INFO("RAW IMU [%d]", raw_imu_counter );
}
raw_imu_counter++;

pronto::atlas_raw_imu_batch_t imu;
imu.utime = (int64_t) floor(msg->header.stamp.toNSec()/1000);

Expand Down Expand Up @@ -413,7 +465,7 @@ void ROS_2_LCM::publish_multisense_state(int64_t utime, float position, float ve

msg_out.joint_position[0] = position;
msg_out.joint_velocity[0] = velocity;
msg_out.joint_name[0] = "hokuyo_joint";
msg_out.joint_name[0] = "motor_joint";

msg_out.joint_name[1] = "pre_spindle_cal_x_joint";
msg_out.joint_name[2] = "pre_spindle_cal_y_joint";
Expand Down

0 comments on commit 7157988

Please sign in to comment.