void RecoverySupervisor::joyCallback(const sensor_msgs::Joy& msg)
{
ROS_INFO_ONCE("joystick received.");
if (demonstrating_)
{
if (msg.buttons.at(finish_demonstration_button_) == 1)
{
ending_demonstration_ = true;
}
}
else if (msg.buttons.at(force_demonstration_button_) == 1)
{
starting_demonstration_ = true;
}
else if (msg.buttons[10] == 1)
{
ROS_INFO("creating new bag");
bag_mutex_.lock();
bag_->close();
bag_index_++;
std::string bag_name = bag_file_directory_ + "/" + std::to_string(bag_index_) + ".bag";
bag_->open(bag_name, rosbag::bagmode::Write);
bag_mutex_.unlock();
}
}
map_t * requestCSpaceMap(const char *srv_name, const int free_threshold,
const int occupied_threshold) {
ros::NodeHandle nh;
// Taken from PAMCL
map_t* map = map_alloc();
ROS_ASSERT(map);
// get map via RPC
nav_msgs::GetMap::Request req;
nav_msgs::GetMap::Response resp;
ROS_INFO("Requesting the map...");
while(!ros::service::call(srv_name, req, resp)) {
ROS_WARN("Request for map '%s' failed; trying again...",
ros::names::resolve(string(srv_name)).c_str());
ros::Duration d(4.0);
d.sleep();
if (!nh.ok()) {
return NULL;
}
}
ROS_INFO_ONCE("Received a %d X %d map @ %.3f m/pix\n",
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution);
convertMap(resp.map, map, free_threshold, occupied_threshold);
return map;
}
// Get the URDF XML from the parameter server
std::string getURDF(std::string param_name) const
{
std::string urdf_string;
// search and wait for robot_description on param server
while (urdf_string.empty())
{
std::string search_param_name;
if (model_nh_.searchParam(param_name, search_param_name))
{
ROS_INFO_ONCE("gazebo_ros_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.", search_param_name.c_str());
model_nh_.getParam(search_param_name, urdf_string);
}
else
{
ROS_INFO("gazebo_ros_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.", robot_description_.c_str());
model_nh_.getParam(param_name, urdf_string);
}
usleep(100000);
}
ROS_INFO("gazebo_ros_control got urdf from param server, parsing...");
return urdf_string;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "publisher");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<workshop_msgs::DetectionsStamped>("face_detections_out", 1000);
ros::Rate loop_rate(10);
while( ros::ok() )
{
workshop_msgs::DetectionsStamped msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "";
msg.detections.type = workshop_msgs::Detections::FACE;
sensor_msgs::RegionOfInterest det;
det.x_offset = 100;
det.y_offset = 100;
det.width = 64;
det.height = 64;
msg.detections.rects.push_back(det);
ROS_INFO_ONCE( "Message sent" );
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
/*
void CtrlInterface::cmdPlanVelCallback(const geometry_msgs::Twist::ConstPtr twist_msg)
{
boost::mutex::scoped_lock(mutex_);
if (!allow_plan_vel_cmd_)
{
ROS_WARN("Received planner velocity command, but allow_plan_vel_cmd is set to false");
return;
}
if (ctrl_type_ != mav::VelocityCtrl)
{
ROS_WARN("Received joystick velocity command, but ctrl_type is not VELOCITY");
return;
}
// TODO: check for frame here
ROS_INFO("Executing velocity command from planner.");
des_vel_ = *twist_msg;
publishCmdVel();
}
*/
void CtrlInterface::curPoseCallback(const geometry_msgs::PoseStamped::ConstPtr pose_msg)
{
boost::mutex::scoped_lock(mutex_);
std::string frame = tf_listener_.resolve(pose_msg->header.frame_id);
if (frame != fixed_frame_)
{
ROS_INFO_ONCE("Pose message does not match the fixed frame (%s, %s), transforming",
frame.c_str(), fixed_frame_.c_str());
try
{
tf_listener_.waitForTransform(
fixed_frame_, pose_msg->header.frame_id, pose_msg->header.stamp, ros::Duration(1.0));
tf_listener_.transformPose(fixed_frame_, *pose_msg, cur_pose_);
}
catch (tf::TransformException ex)
{
ROS_WARN("Could not transform goal pose %s", ex.what());
return;
}
}
else
{
cur_pose_ = *pose_msg;
}
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"goal_pub");
ros::NodeHandle n;
float goal_x ;
float goal_y ;
n.param ("goal_x", goal_x) ;
n.param ("goal_x", goal_y) ;
ros::Publisher goal_pub;
std::string goal_topic_name = "/move_base_simple/goal";
goal_pub = n.advertise<geometry_msgs::PoseStamped>(goal_topic_name.c_str(), 1);
geometry_msgs::PoseStamped goal;
goal.header.frame_id = "map";
goal.pose.position.x = goal_x ;
goal.pose.position.y = goal_y ;
goal.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
ros::Rate r(1);
r.sleep();
for(int i=0;i<1000;i++)
goal_pub.publish(goal);
ROS_INFO_ONCE("Goal was published");
ros::spin();
return 0;
}
void RecoverySupervisor::tfCallback(const tf2_msgs::TFMessage& msg)
{
ROS_INFO_ONCE("tf received.");
bag_mutex_.lock();
bag_->write("tf", ros::Time::now(), msg);
bag_mutex_.unlock();
}
void HANPLayer::humansUpdate(const hanp_msgs::TrackedHumansPtr& humans)
{
update_mutex_.lock();
// store humans to local variable
trackedHumans = humans;
ROS_INFO_ONCE("hanp_layer: subscribed to humans");
update_mutex_.unlock();
}
void RecoverySupervisor::velocityCallback(const geometry_msgs::Twist& msg)
{
ROS_INFO_ONCE("velocity received.");
bag_mutex_.lock();
bag_->write("velocity", ros::Time::now(), msg);
bag_mutex_.unlock();
last_velocity_ = msg;
}
void AssistedSteering::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
ROS_INFO_ONCE("Laser received!");
//IMPORTANT: the frame of the laser should be the center of the robot (base_link)
//Otherwise we should include a shift to the center in the calculations.
laser_mutex_.lock();
laser_scan_ = *msg;
scan_time_ = ros::Time::now();
laser_mutex_.unlock();
}
void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
ROS_INFO_ONCE("odom received!");
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
base_odom_.child_frame_id = msg->child_frame_id;
}
void EstimatorNode::GroundTruthCallback(
const geometry_msgs::PoseStampedConstPtr &pose_msg){
ROS_INFO_ONCE("Ground truth initiated.");
ground_truth_pose(0) = pose_msg->pose.orientation.x;
ground_truth_pose(1) = pose_msg->pose.orientation.y;
ground_truth_pose(2) = pose_msg->pose.orientation.z;
ground_truth_pose(3) = pose_msg->pose.orientation.w;
}
VisionNode::VisionNode() {
cv::FileStorage fs("/home/roboy/workspace/mocap/src/intrinsics.xml", cv::FileStorage::READ);
if (!fs.isOpened()) {
ROS_ERROR("could not open intrinsics.xml");
return;
}
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;
fs.release();
ID = 0;
// calculate undistortion mapping
initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, cv::Size(WIDTH, HEIGHT), 1,
cv::Size(WIDTH, HEIGHT), 0),
cv::Size(WIDTH, HEIGHT), CV_16SC2, map1, map2);
marker_position_pub = new ros::Publisher;
*marker_position_pub = nh.advertise<communication::MarkerPosition>("/mocap/marker_position", 100);
video_pub = new ros::Publisher;
*video_pub = nh.advertise<sensor_msgs::Image>("/mocap/video", 1);
camera_control_sub = nh.subscribe("/mocap/camera_control", 100, &VisionNode::camera_control, this);
cameraID_pub = new ros::Publisher;
*cameraID_pub = nh.advertise<std_msgs::Int32>("/mocap/cameraID", 100);
// Publish the marker
while (cameraID_pub->getNumSubscribers() < 1) {
ros::Duration d(1.0);
if (!ros::ok()) {
return;
}
ROS_WARN_ONCE("Waiting for mocap plugin to subscribe to /mocap/cameraID");
d.sleep();
}
ROS_INFO_ONCE("Found subscriber");
spinner = new ros::AsyncSpinner(1);
spinner->start();
std_msgs::Int32 msg;
msg.data = ID;
cameraID_pub->publish(msg);
img = cv::Mat(HEIGHT, WIDTH, CV_8UC4, img_data);
img_rectified = cv::Mat(HEIGHT, WIDTH, CV_8UC4, img_rectified_data);
t1 = std::chrono::high_resolution_clock::now();
StartCamera(WIDTH, HEIGHT, 90, CameraCallback);
}
/*****************************************************************************************************************************************
Start of main
********************************************************************************************************************************************/
int main(int argc, char** argv){
//init
ros::init(argc, argv, "joystick_to_twist_node");
ROS_INFO_ONCE("ROS is initialized");
Subscriber sub;
//set frame rate for ROS while loop
ros::Rate loop_rate(ROS_FRAME_RATE);
while(ros::ok())
{
sub.sendValue();
ROS_INFO_ONCE("first value is send");
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
void OdometryHelperRos::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
ROS_INFO_ONCE("odom received!");
//we assume that the odometry is published in the frame of the base
boost::mutex::scoped_lock lock(odom_mutex_);
base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
base_odom_.child_frame_id = msg->child_frame_id;
// ROS_DEBUG_NAMED("dwa_local_planner", "In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
// base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
}
void FlyerInterface::cmdPoseCallback(const geometry_msgs::PoseStamped::ConstPtr cmd_pose_msg)
{
// TODO
ros::Duration tf_tol(5.0);
// **** transform the message to the correct frame
std::string frame = tf_listener_.resolve(cmd_pose_msg->header.frame_id);
geometry_msgs::PoseStamped cmd_pose_fixed_frame;
if (frame != fixed_frame_)
{
ROS_INFO_ONCE("cmd_pose messages does not match the fixed frame (%s, %s), transforming",
frame.c_str(), fixed_frame_.c_str());
try
{
tf_listener_.waitForTransform(
fixed_frame_, cmd_pose_msg->header.frame_id, cmd_pose_msg->header.stamp, tf_tol);
tf_listener_.transformPose(fixed_frame_, *cmd_pose_msg, cmd_pose_fixed_frame);
}
catch (tf::TransformException ex)
{
ROS_WARN("Could not transform goal pose %s", ex.what());
return;
}
}
else
{
cmd_pose_fixed_frame = *cmd_pose_msg;
}
MAV_DES_POSE_PKT des_pose_pkt;
des_pose_pkt.x = cmd_pose_fixed_frame.pose.position.x;
des_pose_pkt.y = cmd_pose_fixed_frame.pose.position.y;
des_pose_pkt.z = cmd_pose_fixed_frame.pose.position.z;
float cmd_yaw = tf::getYaw(cmd_pose_fixed_frame.pose.orientation);
normalizeSIAnglePi(&cmd_yaw);
des_pose_pkt.yaw = cmd_yaw;
ROS_DEBUG("Sending MAV_DES_POSE_PKT packet");
comm_.sendPacket(MAV_DES_POSE_PKT_ID, des_pose_pkt);
// **** publish debug cmd yaw
std_msgs::Float32 cmd_yaw_msg;
cmd_yaw_msg.data = cmd_yaw;
debug_cmd_yaw_publisher_.publish(cmd_yaw_msg);
}
void RecoverySupervisor::moveBaseStatusCallback(const actionlib_msgs::GoalStatusArray& msg)
{
ROS_INFO_ONCE("Movebase status received.");
if (first_msg_)
{
first_msg_ = false;
if (msg.status_list.size() > 0)
{
current_movebase_goal_ = msg.status_list[0].goal_id.id;
}
}
actionlib_msgs::GoalStatus oldest_msg = msg.status_list[0];
int status = oldest_msg.status;
std::string goal_id = oldest_msg.goal_id.id;
if (status == actionlib_msgs::GoalStatus::ABORTED)
{
if (has_goal_)
{
ROS_WARN("Goal was aborted.");
starting_demonstration_ = true;
}
has_goal_ = false;
current_movebase_goal_ = goal_id;
}
else if (status == actionlib_msgs::GoalStatus::SUCCEEDED)
{
if (current_movebase_goal_ != goal_id)
{
has_goal_ = false;
// we successfully reached our goal! bag it.
ROS_DEBUG("Goal reached!");
bag_mutex_.lock();
bag_->write("goals_reached", ros::Time::now(), latest_pose_);
bag_mutex_.unlock();
// now check if that took too long compared to our trip_time
auto actual_trip_time = ros::Time::now() - trip_time_start_time_;
auto max_allowed_trip_time = estimate_trip_time_ * max_trip_time_error_factor_;
if (actual_trip_time > max_allowed_trip_time)
{
ROS_WARN("max allow time: %fs, actual time: %fs", max_allowed_trip_time.toSec(), actual_trip_time.toSec());
// starting_demonstration_ = true;
}
current_movebase_goal_ = goal_id;
}
}
}
void run() {
ros::Rate loop_rate(rate);
while (ros::ok())
{
ROS_INFO_ONCE("MMS: RUNNING");
MMS_Handle();
counter_++;
ros::spinOnce();
loop_rate.sleep();
}
}
void EstimatorNode::PoseCallback(
const geometry_msgs::PoseStampedConstPtr& pose_msg) {
ROS_INFO_ONCE("Estimator got first pose message.");
msgPose_.header.stamp = pose_msg->header.stamp;
msgPose_.header.seq = pose_msg->header.seq;
msgPose_.header.frame_id =pose_msg->header.frame_id;
// Update measurement vector
z(0) = pose_msg->pose.position.x;
z(1) = pose_msg->pose.position.y;
z(2) = pose_msg->pose.position.z;
// Slide 6: Update or correction (based on measurements)
K = P_pred*H.transpose()*((H*P_pred*H.transpose() + R)).inverse();
x = x_pred + K*(z-z_pred);
P = P_pred - K*H*P_pred;
}
void RecoverySupervisor::odometryCallback(const nav_msgs::Odometry& msg)
{
ROS_INFO_ONCE("odom received.");
//bag_mutex_.lock();
//bag_->write("odom", ros::Time::now(), msg);
//bag_mutex_.unlock();
latest_pose_.pose = msg.pose.pose;
latest_pose_.header = msg.header;
double displacement_since_last_recovery = dist(latest_pose_, last_recovery_pose_);
// if we've moved far enough, we must be making progress so reset recovery count
if (displacement_since_last_recovery > minimum_displacement_)
{
first_recovery_count_ = 0;
}
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
ROS_INFO_STREAM_ONCE("Image received.");
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
cv::Mat frame_rgb = cv_ptr->image;
cv::Mat frame_gray;
cv::cvtColor( frame_rgb, frame_gray, CV_BGR2GRAY );
cv::equalizeHist( frame_gray, frame_gray );
std::vector<cv::Rect> faces;
face_cascade.detectMultiScale( frame_gray, faces, 1.5, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
// Prepare and publish all the detections
workshop_msgs::DetectionsStamped msg_out;
msg_out.header.stamp = msg->header.stamp;
msg_out.header.frame_id = msg->header.frame_id;
msg_out.detections.type = workshop_msgs::Detections::FACE;
for( int i = 0; i < (int)faces.size(); i++ )
{
sensor_msgs::RegionOfInterest det;
det.x_offset = faces[i].x;
det.y_offset = faces[i].y;
det.width = faces[i].width;
det.height = faces[i].height;
msg_out.detections.rects.push_back(det);
}
pub.publish(msg_out);
ROS_INFO_ONCE( "Message sent" );
// Show the detections using OpenCV window
// for( int i = 0; i < (int)faces.size(); i++ )
// {
// cv::rectangle( frame_rgb, faces[i], cv::Scalar( 0, 255, 0 ), 4, 8, 0 );
// }
//
// cv::imshow( "Image from Topic", frame_rgb );
// cv::waitKey(10);
}
void LeePositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("LeePositionController got first odometry message.");
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
lee_position_controller_.SetOdometry(odometry);
Eigen::VectorXd ref_rotor_velocities;
lee_position_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
// Todo(ffurrer): Do this in the conversions header.
mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators);
actuator_msg->angular_velocities.clear();
for (int i = 0; i < ref_rotor_velocities.size(); i++)
actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]);
actuator_msg->header.stamp = odometry_msg->header.stamp;
motor_velocity_reference_pub_.publish(actuator_msg);
}
void RollPitchYawrateThrustControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("RollPitchYawrateThrustController got first odometry message.");
EigenOdometry odometry;
eigenOdometryFromMsg(odometry_msg, &odometry);
roll_pitch_yawrate_thrust_controller_.SetOdometry(odometry);
Eigen::VectorXd ref_rotor_velocities;
roll_pitch_yawrate_thrust_controller_.CalculateRotorVelocities(&ref_rotor_velocities);
// Todo(ffurrer): Do this in the conversions header.
mav_msgs::CommandMotorSpeedPtr turning_velocities_msg(new mav_msgs::CommandMotorSpeed);
turning_velocities_msg->motor_speed.clear();
for (int i = 0; i < ref_rotor_velocities.size(); i++)
turning_velocities_msg->motor_speed.push_back(ref_rotor_velocities[i]);
turning_velocities_msg->header.stamp = odometry_msg->header.stamp;
motor_velocity_reference_pub_.publish(turning_velocities_msg);
}
void EstimatorNode::ImuCallback(
const sensor_msgs::ImuConstPtr& imu_msg) {
ROS_INFO_ONCE("Estimator got first IMU message.");
msgPose_.header.stamp = imu_msg->header.stamp;
msgPose_.header.seq = imu_msg->header.seq;
msgPose_.header.frame_id =imu_msg->header.frame_id;
// Update input vector
u(0) = imu_msg->linear_acceleration.x;
u(1) = imu_msg->linear_acceleration.y;
// Fix gravity or the altitude goes rock&roll crazy :)
u(2) = imu_msg->linear_acceleration.z - 9.81;
// Prediction (based on model)
x_pred = F*x + G*u;
P_pred = F*P*F.transpose() + Q;
z_pred = H*x_pred;
}
void RecoverySupervisor::amclCallback(const geometry_msgs::PoseWithCovarianceStamped& msg)
{
ROS_INFO_ONCE("amcl received.");
//bag_mutex_.lock();
//bag_->write("amcl_pose", ros::Time::now(), msg);
//bag_mutex_.unlock();
if (first_amcl_msg_)
{
first_amcl_msg_ = false;
}
else
{
// check for localization jumps
geometry_msgs::PoseStamped curent_pose;
curent_pose.pose = msg.pose.pose;
curent_pose.header = msg.header;
double displacement = dist(curent_pose, last_amcl_pose_);
ROS_DEBUG("displacement %f", displacement);
if (displacement > maximum_displacement_jump_)
{
ROS_ERROR("Localization failure: jumped by %f meters", displacement);
actionlib_msgs::GoalID msg;
msg.stamp = ros::Time(0);
cancel_pub_.publish(msg);
}
}
last_amcl_pose_.pose = msg.pose.pose;
last_amcl_pose_.header = msg.header;
if (!demonstrating_)
{
// assuming we haven't failed yet,
// record our position as nav_msgs/Path
current_amcl_path_.poses.push_back(last_amcl_pose_);
}
}
void EstimatorNode::TimedCallback(
const ros::TimerEvent& e){
ROS_INFO_ONCE("Timer initiated.");
deltaT = ros::Time::now().toSec() - lastTime;
deltaTsq = deltaT*deltaT;
deltaTcu = deltaT*deltaT*deltaT;
// See slide 16
F << 1, 0, 0, deltaT, 0, 0,
0, 1, 0, 0, deltaT, 0,
0, 0, 1, 0, 0, deltaT,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, 1;
// See slide 18
G << deltaTsq/2, 0, 0,
0, deltaTsq/2, 0,
0, 0, deltaTsq/2,
deltaT, 0, 0,
0, deltaT, 0,
0, 0, deltaT;
// See slide 19
Q << deltaTcu/2, 0, 0, deltaTsq/2, 0, 0,
0, deltaTcu/2, 0, 0, deltaTsq/2, 0,
0, 0, deltaTcu/2, 0, 0, deltaTsq/2,
deltaTsq/2, 0, 0, deltaTsq/2, 0, 0,
0, deltaTsq/2, 0, 0, deltaTsq/2, 0,
0, 0, deltaTsq/2, 0, 0, deltaTsq/2;
Q = sigmaXsq * Q;
lastTime = ros::Time::now().toSec();
Publish();
}
void RecoverySupervisor::newGoalCallback(const std_msgs::Int32& msg)
{
ROS_INFO_ONCE("nav goal recieved.");
has_goal_ = true;
has_path_ = false;
bag_mutex_.lock();
bag_->write("nav_points/goal_id", ros::Time::now(), msg);
bag_mutex_.unlock();
current_goal_id_ = msg.data;
// reset current demo and related messages
if (!demonstrating_)
{
current_demo_.header.stamp = ros::Time::now();
current_demo_.feature_values.clear();
current_amcl_path_.poses.clear();
current_amcl_path_.header.stamp = ros::Time::now();
current_amcl_path_.header.frame_id = "map";
}
trip_time_start_time_ = ros::Time::now();
}
void odometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) {
ROS_INFO_ONCE("[pos_ctl_nd] First odometry message received");
rotors_control::EigenOdometry odometry;
rotors_control::eigenOdometryFromMsg(odometry_msg, &odometry);
position_controller.SetOdometry(odometry);
position_controller.RunController();
thrust_msg.header.stamp = odometry_msg->header.stamp;
thrust_msg.thrust.z = position_controller._thrust[2];
thrust_msg.roll = position_controller._RPY_SP[0];
thrust_msg.pitch = position_controller._RPY_SP[1];
std::cout << position_controller._thrust << std::endl;
std::cout << "____" << std::endl;
std::cout << position_controller._RPY_SP[0]<<" " << position_controller._RPY_SP[1] << std::endl;
std::cout << "_____________________" << std::endl;
chatter_pub.publish(thrust_msg);
}
// updateJoints
//
// std_msgs/Header header
// uint32 seq
// time stamp
// string frame_id
// string[] name
// ---
// float64[] commanded_position
// float64[] commanded_velocity
// float64[] commanded_acceleration
// float64[] commanded_effort
// ---
// float64[] actual_position // Current joint angle position in radians
// float64[] actual_velocity
// float64[] actual_effort // This includes the inertial feed forward torques when applicable.
// float64[] gravity_model_effort // This is the torque required to hold the arm against gravity returned by KDL if the arm was stationary.
// // This does not include inertial feed forward torques (even when we have them) or any of the corrections (i.e. spring
// hysteresis, crosstalk, etc) we make to the KDL model.
// float64[] gravity_only
// float64[] hysteresis_model_effort
// float64[] crosstalk_model_effort
// float64 hystState
// -------------------------------------------------------------------------------
void controller::updateJoints(const baxter_core_msgs::SEAJointStateConstPtr& state)
{
// Local variables
ros::Time t = ros::Time::now();
unsigned int i, k=0;
// Joint, Torque, and Gravitation Torque values
std::vector<double> jt, jtf, tt, tg, ttf, tgf;
// Joint Angles
jt.clear(); jtf.clear();
jt.resize(joints_names_.size());
jtf.resize(joints_names_.size());
// Regular Torques
tt.resize(joints_names_.size());
ttf.resize(joints_names_.size());
// Gravitational Torques
tg.resize(joints_names_.size());
tgf.resize(joints_names_.size());
// Update the current joint values, actual effort, and gravity_model_effort values
ROS_INFO_ONCE("Initializing joints");
if(exe_ && save_.is_open())
save_ << (t - to_).toSec() << " ";
while(k < joints_names_.size())
{
for(i=0; i<state->name.size(); i++)
{
if(state->name[i] == joints_names_[k])
{
jt[k] = state->actual_position[i];
tt[k] = state->actual_effort[i];
tg[k] = state->gravity_model_effort[i];
if(exe_ && save_.is_open())
save_ << state->actual_position[i] << " ";
k = k + 1;
if(k == joints_names_.size())
break;
}
}
}
if(!jo_ready_)
{
// Save current values into previous values
for(unsigned int i=0; i<joints_names_.size(); i++)
{
ttf[i] = tt[i];
tgf[i] = tg[i];
// Create previous values
tm_t_1_[i] = tt[i];
tg_t_1_[i] = tg[i];
j_t_1_[i] = jt[i];
}
// Savlue previous values
torque_.push_back(tm_t_1_);
torque_.push_back(tm_t_1_);
tg_.push_back(tg_t_1_);
tg_.push_back(tg_t_1_);
joints_.push_back(jt);
joints_.push_back(jt);
jo_ready_ = true;
}
// Compute the offsets
for(unsigned int i=0; i<7; i++)
{
tgf[i] = 0.0784*tg_t_1_[i] + 1.5622*tg_[0][i] - 0.6413*tg_[1][i];
ttf[i] = 0.0784*tm_t_1_[i] + 1.5622*torque_[0][i] - 0.6413*torque_[1][i];
jtf[i] = 0.0784*j_t_1_[i] + 1.5622*joints_[0][i] - 0.6413*joints_[1][i];
tm_t_1_[i] = tt[i];
tg_t_1_[i] = tg[i];
j_t_1_[i] = jt[i];
tg_[1][i] = tg_[0][i];
tg_[0][i] = tgf[i];
torque_[1][i] = torque_[0][i];
torque_[0][i] = ttf[i];
joints_[1][i] = joints_[0][i];
joints_[0][i] = jtf[i];
}
if(exe_ && save_.is_open())
{
for(unsigned int j=0; j<joints_.size(); j++)
save_ << tt[j] << " ";
for(unsigned int j=0; j<joints_.size(); j++)
save_ << tg[j] << " ";
save_ << std::endl;
}
ROS_INFO_STREAM_ONCE("Joints updated, tor: " << ttf[0] << ", "<< ttf[1] << ", "<< ttf[2]);
}
void MotorController::setPID()
{
ros::Rate rate(frequency_);
ROS_INFO_STREAM("Setting PID Values:"
<< "\n\t P => L: " << p_l_ << " R: " << p_r_ << "\n\t D => L: " << d_l_ << " R: " << d_r_
<< "\n\t I => L: " << i_l_ << " R: " << i_r_ << "\n\t Kv => L: " << kv_l_ << " R: " << kv_r_);
bool valid_values = false; // pid values have been set correctly
/* This is the buffer where we will store the request message. */
uint8_t requestbuffer[256];
size_t message_length;
bool status;
/* allocate space for the request message to the server */
RequestMessage request = RequestMessage_init_zero;
/* Create a stream that will write to our buffer. */
pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer));
/* indicate that pid fields will contain values */
request.has_p_l = true;
request.has_p_r = true;
request.has_i_l = true;
request.has_i_r = true;
request.has_d_l = true;
request.has_d_r = true;
request.has_kv_l = true;
request.has_kv_r = true;
/* fill in the message fields */
request.p_l = static_cast<float>(p_l_);
request.p_r = static_cast<float>(p_r_);
request.i_l = static_cast<float>(i_l_);
request.i_r = static_cast<float>(i_r_);
request.d_l = static_cast<float>(d_l_);
request.d_r = static_cast<float>(d_r_);
request.kv_l = static_cast<float>(kv_l_);
request.kv_r = static_cast<float>(kv_r_);
/* encode the protobuffer */
status = pb_encode(&ostream, RequestMessage_fields, &request);
message_length = ostream.bytes_written;
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream));
ros::shutdown();
}
size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read
uint8_t buffer[256]; // buffer to read response into
// unsigned char responsebuffer[256];
/* Send PID values via ethernet and recieve response to ensure proper setting */
while (ros::ok() && !valid_values)
{
(*sock_).sendMessage(reinterpret_cast<char*>(requestbuffer), message_length);
memset(buffer, 0, sizeof(buffer));
n = (*sock_).readMessage(buffer); // blocks until data is read
// memcpy(responsebuffer, buffer, sizeof(responsebuffer));
if (n == 0)
{
ROS_ERROR_STREAM("Connection closed by server");
ros::shutdown();
}
/* Allocate space for the decoded message. */
ResponseMessage response = ResponseMessage_init_zero;
/* Create a stream that reads from the buffer. */
pb_istream_t istream = pb_istream_from_buffer(buffer, n);
/* decode the message. */
status = pb_decode(&istream, ResponseMessage_fields, &response);
/* check for any errors.. */
if (!status)
{
ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream));
ros::shutdown();
}
valid_values = (response.p_l == static_cast<float>(p_l_)) && (response.p_r == static_cast<float>(p_r_)) &&
(response.i_l == static_cast<float>(i_l_)) && (response.i_r == static_cast<float>(i_r_)) &&
(response.d_l == static_cast<float>(d_l_)) && (response.d_r == static_cast<float>(d_r_)) &&
(response.kv_l == static_cast<float>(kv_l_)) && (response.kv_r == static_cast<float>(kv_r_));
rate.sleep();
}
ROS_INFO_ONCE("Sucessfully set all PID values");
}
