// Update the controller
void GazeboRosSkidSteerDrive::UpdateChild() {
common::Time current_time = this->world->GetSimTime();
double seconds_since_last_update =
(current_time - last_update_time_).Double();
if (seconds_since_last_update > update_period_) {
publishOdometry(seconds_since_last_update);
// Update robot in case new velocities have been requested
getWheelVelocities();
#if GAZEBO_MAJOR_VERSION > 2
joints[LEFT_FRONT]->SetParam("vel", 0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0));
joints[RIGHT_FRONT]->SetParam("vel", 0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0));
joints[LEFT_REAR]->SetParam("vel", 0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0));
joints[RIGHT_REAR]->SetParam("vel", 0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0));
#else
joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0));
joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0));
joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0));
joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0));
#endif
last_update_time_+= common::Time(update_period_);
}
}
void ExcavaROBOdometry::update()
{
if (!isInputValid()) {
if (verbose_)
debug_publisher_->msg_.input_valid = false;
ROS_DEBUG("Odometry: Input velocities are invalid");
return;
}
else {
if (verbose_)
debug_publisher_->msg_.input_valid = true;
}
current_time_ = base_kin_.robot_state_->getTime();
ros::Time update_start = ros::Time::now();
updateOdometry();
// double update_time = (ros::Time::now()-update_start).toSec();
ros::Time publish_start = ros::Time::now();
if (publish_odom_)
publishOdometry();
if (publish_odometer_)
publishOdometer();
if (publish_state_odometry_)
publishStateOdometry();
if (publish_tf_)
publishTransform();
last_r_wheel_rotation_ = current_r_wheel_rotation_;
last_l_wheel_rotation_ = current_l_wheel_rotation_;
last_time_ = current_time_;
sequence_ ++;
}
// Update the controller
void GazeboRosDiffDriveMultiWheel::UpdateChild() {
common::Time current_time = this->world->GetSimTime();
double seconds_since_last_update =
(current_time - last_update_time_).Double();
if (seconds_since_last_update > update_period_) {
if (this->publish_odometry_tf_ || this->publish_odometry_msg_){
publishOdometry(seconds_since_last_update);
}
// Update robot in case new velocities have been requested
getWheelVelocities();
//joints[LEFT]->SetVelocity(0, wheel_speed_[LEFT] / wheel_diameter_);
//joints[RIGHT]->SetVelocity(0, wheel_speed_[RIGHT] / wheel_diameter_);
for (size_t side = 0; side < 2; ++side){
for (size_t i = 0; i < joints_[side].size(); ++i){
joints_[side][i]->SetVelocity(0, wheel_speed_[side] / (0.5 * wheel_diameter_));
}
}
last_update_time_+= common::Time(update_period_);
}
}
// Set and publish full state.
void Publisher::publishFullStateAsCallback(
const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
const Eigen::Matrix<double, 9, 1> & speedAndBiases,
const Eigen::Matrix<double, 3, 1> & omega_S)
{
setTime(t);
setOdometry(T_WS, speedAndBiases, omega_S); // TODO: provide setters for this hack
setPath(T_WS);
publishOdometry();
publishTransform();
publishPath();
}
void VizHelperNode::publishSat(const iri_common_drivers_msgs::SatellitePseudorange &sat, ros::Time time_ros)
{
visualization_msgs::Marker m;
m.header.frame_id = WORLD_FRAME;
m.header.stamp = time_ros;
m.ns = "sats";
m.id = sat.sat_id;
m.type = visualization_msgs::Marker::CUBE;//SPHERE;
m.action = visualization_msgs::Marker::ADD;
// Pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
m.pose.position.x = sat.x * scale;
m.pose.position.y = sat.y * scale;
m.pose.position.z = sat.z * scale;
m.pose.orientation.x = 0.0;
m.pose.orientation.y = 0.0;
m.pose.orientation.z = 0.0;
m.pose.orientation.w = 1.0; // todo dovrei mettere la stessa posa del suo frame
m.scale.x = m.scale.y = m.scale.z = 1000;
m.color.r = 1.0f;
m.color.g = 0.0f;
m.color.b = 0.0f;
m.color.a = 0.5;
m.lifetime = ros::Duration(LIFETIME_SHORT); //after tot seconds satellites are deleted
markerPub.publish(m);
publishSatVelocity(sat, time_ros); // publish velocity
// calculate rotation quaternion.
// rotate (vedi cosa va la funzione rotateSatelliteFrame)
Eigen::Quaterniond rotation = rotateSatelliteFrame(sat, time_ros);
// publish odometry
publishOdometry(sat, rotation, time_ros);
// publish earth vector
//TODO
// publish sat sphere
publishSatSphere(sat, time_ros);
}
// Update the controller
void GazeboRosDiffDrive::UpdateChild()
{
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
// Update robot in case new velocities have been requested
getWheelVelocities();
double current_speed[2];
current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
if ( wheel_accel == 0 ||
( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) ||
( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) {
//if max_accel == 0, or target speed is reached
joints_[LEFT]->SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
} else {
if ( wheel_speed_[LEFT]>=current_speed[LEFT] )
wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
if ( wheel_speed_[RIGHT]>current_speed[RIGHT] )
wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
joints_[LEFT]->SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
}
last_update_time_+= common::Time ( update_period_ );
}
}
// Update the controller
void GazeboRosPlanarMove::UpdateChild()
{
boost::mutex::scoped_lock scoped_lock(lock);
math::Pose pose = parent_->GetWorldPose();
float yaw = pose.rot.GetYaw();
parent_->SetLinearVel(math::Vector3(
x_ * cosf(yaw) - y_ * sinf(yaw),
y_ * cosf(yaw) + x_ * sinf(yaw),
0));
parent_->SetAngularVel(math::Vector3(0, 0, rot_));
if (odometry_rate_ > 0.0) {
common::Time current_time = parent_->GetWorld()->GetSimTime();
double seconds_since_last_update =
(current_time - last_odom_publish_time_).Double();
if (seconds_since_last_update > (1.0 / odometry_rate_)) {
publishOdometry(seconds_since_last_update);
last_odom_publish_time_ = current_time;
}
}
}
// Update the controller
void GazeboRosTricycleDrive::UpdateChild()
{
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 );
double target_steering_angle = cmd_.angle;
motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update );
//ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);
last_actuator_update_ += common::Time ( update_period_ );
}
}
// Update the controller
void GazeboRosDiffDrive::UpdateChild()
{
/* force reset SetMaxForce since Joint::Reset reset MaxForce to zero at
https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331
(this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 )
and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset
(this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e)
*/
for ( int i = 0; i < 2; i++ ) {
if ( fabs(wheel_torque -joints_[i]->GetParam ("fmax", 0 )) > 1e-6 ) {
joints_[i]->SetParam("fmax", 0, wheel_torque );
}
}
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
// Update robot in case new velocities have been requested
getWheelVelocities();
double current_speed[2];
current_speed[LEFT ] = joints_[LEFT ]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
if ( wheel_accel == 0 ||
( ( fabs ( wheel_speed_[LEFT ] - current_speed[LEFT ] ) < 0.01 ) &&
( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) ) {
//if max_accel == 0, or target speed is reached
wheel_speed_instr_[LEFT ] = wheel_speed_[LEFT ];
wheel_speed_instr_[RIGHT] = wheel_speed_[RIGHT];
} else {
if ( wheel_speed_[LEFT ] >= current_speed[LEFT ] )
wheel_speed_instr_[LEFT ] += fmin ( wheel_speed_[LEFT ]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[LEFT ] += fmax ( wheel_speed_[LEFT ]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
if ( wheel_speed_[RIGHT] > current_speed[RIGHT] )
wheel_speed_instr_[RIGHT] += fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[RIGHT] += fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
// ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
}
wheel_applied_vel[LEFT ] = wheel_speed_instr_[LEFT ] / ( wheel_diameter_ / 2.0 );
wheel_applied_vel[RIGHT] = wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 );
last_update_time_ = current_time;
joints_[LEFT ]->SetParam("vel", 0, wheel_applied_vel[LEFT ] );
joints_[RIGHT]->SetParam("vel", 0, wheel_applied_vel[RIGHT] );
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "wf_simulator");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
std::string initialize_source;
private_nh.getParam("initialize_source", initialize_source);
ROS_INFO_STREAM("initialize_source : " << initialize_source);
double accel_rate;
private_nh.param("accel_rate",accel_rate,double(1.0));
ROS_INFO_STREAM("accel_rate : " << accel_rate);
// publish topic
g_odometry_publisher = nh.advertise<geometry_msgs::PoseStamped>("sim_pose", 10);
g_velocity_publisher = nh.advertise<geometry_msgs::TwistStamped>("sim_velocity", 10);
// subscribe topic
ros::Subscriber cmd_subscriber = nh.subscribe<geometry_msgs::TwistStamped>("twist_cmd", 10, boost::bind(CmdCallBack, _1, accel_rate));
ros::Subscriber waypoint_subcscriber = nh.subscribe("base_waypoints", 10, waypointCallback);
ros::Subscriber initialpose_subscriber;
if (initialize_source == "Rviz")
{
initialpose_subscriber = nh.subscribe("initialpose", 10, initialposeCallback);
}
else if (initialize_source == "ndt_localizer")
{
initialpose_subscriber = nh.subscribe("ndt_pose", 10, callbackFromPoseStamped);
}
else if (initialize_source == "GNSS")
{
initialpose_subscriber = nh.subscribe("gnss_pose", 10, callbackFromPoseStamped);
}
else
{
ROS_INFO("Set pose initializer!!");
}
ros::Rate loop_rate(LOOP_RATE);
while (ros::ok())
{
ros::spinOnce(); // check subscribe topic
if (!_waypoint_set)
{
loop_rate.sleep();
continue;
}
if (!_initial_set)
{
loop_rate.sleep();
continue;
}
publishOdometry();
loop_rate.sleep();
}
return 0;
}
// Update the controller
void GazeboRosCarDrive::UpdateChild()
{
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_update_time_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
if ( publishJointTF_ ) publishJointTF();
if ( publishJointState_ ) publishJointStates();
// Update robot in case new velocities have been requested
getJointVelocityCmd();
//////////////////////////////////////////////////////////////////////////////////////////////
#ifdef USE_STEER_PID
this->parent->GetJointController()->SetPositionTarget(joints_[LEFT_FRONT_STEER]->GetScopedName(), steer_angle_cmd_[LEFT]);
this->parent->GetJointController()->SetPositionTarget(joints_[RIGHT_FRONT_STEER]->GetScopedName(), steer_angle_cmd_[RIGHT]);
this->parent->GetJointController()->Update();
#else
joints_[LEFT_FRONT_STEER]->SetPosition( 0, steer_angle_cmd_[LEFT]);
joints_[RIGHT_FRONT_STEER]->SetPosition ( 0, steer_angle_cmd_[RIGHT]);
#endif
//////////////////////////////////////////////////////////////////////////////////////////////
// wheel_speed_cmd_ in rad/s
/*
joints_[LEFT_FRONT_WHEEL]->SetVelocity ( 0, wheel_speed_cmd_[LEFT_FRONT_WHEEL]);
joints_[RIGHT_FRONT_WHEEL]->SetVelocity ( 0, wheel_speed_cmd_[RIGHT_FRONT_WHEEL]);
joints_[LEFT_REAR_WHEEL]->SetVelocity ( 0, wheel_speed_cmd_[LEFT_REAR_WHEEL]);
joints_[RIGHT_REAR_WHEEL]->SetVelocity ( 0, wheel_speed_cmd_[RIGHT_REAR_WHEEL]);
*/
//////////////////////////////////////////////////////////////////////////////////////////////
#ifdef SET_WORLD_SPEED
world_pose_ = this->parent->GetWorldPose();
linear_vel_cmd_.x = cosf(world_pose_.rot.GetYaw())*base_speed_cmd_;
linear_vel_cmd_.y = sinf(world_pose_.rot.GetYaw())*base_speed_cmd_;
angular_vel_cmd_.z = base_omega_cmd_;
this->parent->SetLinearVel(linear_vel_cmd_);
this->parent->SetAngularVel(angular_vel_cmd_);
#endif
last_update_time_+= common::Time ( update_period_ );
//////////////////////////////////////////////////////////////////////////////////////////////
// Debug output
//steer_angle = joints_[LEFT_FRONT_STEER]->GetAngle ( 0 );
ROS_INFO("yaw = %f", world_pose_.rot.GetYaw());
ROS_INFO("base speed = %f", base_speed_cmd_);
ROS_INFO("x = %f", linear_vel_cmd_.x);
ROS_INFO("y = %f", linear_vel_cmd_.y);
//steer_angle = joints_[RIGHT_FRONT_STEER]->GetAngle ( 0 );
//ROS_INFO("steer_angle_cmd = %f, steer_angle = %f",steer_angle_cmd_[RIGHT], steer_angle.Radian());
//ROS_INFO("wheel_speed_cmd = %f, wheel_speed = %f",wheel_speed_cmd_[LEFT_REAR_WHEEL], joints_[LEFT_REAR_WHEEL]->GetVelocity ( 0 ));
//ROS_INFO("wheel_speed_cmd [RL,RR] = [%f,%f], wheel_speed [RL,RR] = [%f,%f]",
// wheel_speed_cmd_[LEFT_REAR_WHEEL]*( wheel_diameter_ / 2.0 ), wheel_speed_cmd_[RIGHT_REAR_WHEEL]*( wheel_diameter_ / 2.0 ),
// joints_[LEFT_REAR_WHEEL]->GetVelocity ( 0 )*( wheel_diameter_ / 2.0 ),joints_[RIGHT_REAR_WHEEL]->GetVelocity ( 0 )*( wheel_diameter_ / 2.0 ));
}
}
// Update the controller
void GazeboRosDiffDrive::UpdateChild() //this is Tick();
{
/* force reset SetParam("fmax") since Joint::Reset reset MaxForce to zero at
https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331
(this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 )
and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset
(this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e)
*/
#ifdef USE_GAZEBO
for ( int i = 0; i < 2; i++ ) {
#if GAZEBO_MAJOR_VERSION > 2
if ( fabs(wheel_torque -joints_[i]->GetParam ( "fmax", 0 )) > 1e-6 ) {
joints_[i]->SetParam ( "fmax", 0, wheel_torque );
#else
if ( fabs(wheel_torque -joints_[i]->GetMaxForce ( 0 )) > 1e-6 ) {
joints_[i]->SetMaxForce ( 0, wheel_torque );
#endif
}
}
#endif
if (odom_source_ == ENCODER)
{
UpdateOdometryEncoder();
// ROS_INFO_NAMED("interop", "jsize:%d",joints_.size());
}
// common::Time current_time = parent->GetWorld()->GetSimTime();
ros::Time current_time = getSimTime();
double seconds_since_last_update = (current_time - last_update_time_).toSec(); // .Double(); //greg
ROS_INFO_NAMED("interop", "UPDATE CHILD, since last update:%f ct:%f lu:%f", seconds_since_last_update, current_time.toSec() , last_update_time_.toSec());
if ( seconds_since_last_update > update_period_ )
{
if (this->publish_tf_) publishOdometry ( seconds_since_last_update );
// if ( publishWheelTF_ ) publishWheelTF(); //greg
// if ( publishWheelJointState_ ) publishWheelJointState();
// Update robot in case new velocities have been requested
getWheelVelocities();
double current_speed[2];
current_speed[LEFT] = joints_[LEFT].GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
current_speed[RIGHT] = joints_[RIGHT].GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 );
if ( wheel_accel == 0 ||
( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) ||
( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) {
//if max_accel == 0, or target speed is reached
#if GAZEBO_MAJOR_VERSION > 2
joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
#else
joints_[LEFT].SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT].SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) );
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop", "^actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop", "^actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT], wheel_speed_[RIGHT]);
#endif
#endif
} else {
if ( wheel_speed_[LEFT]>=current_speed[LEFT] )
wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update );
if ( wheel_speed_[RIGHT]>current_speed[RIGHT] )
wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update );
else
wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update );
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop","*actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop","*actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]);
#endif
#if GAZEBO_MAJOR_VERSION > 2
joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
#else
joints_[LEFT].SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) );
joints_[RIGHT].SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) );
#endif
#ifdef WITH_LOGGING
ROS_INFO_NAMED("interop", "+actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]);
ROS_INFO_NAMED("interop", "+actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT], wheel_speed_[RIGHT]);
#endif
}
// last_update_time_+= common::Time ( update_period_ );//greg
last_update_time_ = ros::Time::now();
last_update_time_ = last_update_time_ + ros::Duration(update_period_); //is this right? seems to work but not tested with period > 0
}
}
// Finalize the controller
void GazeboRosDiffDrive::FiniChild()
{
ROS_INFO_NAMED("interop", "Finichild called");
cmd_vel_subscriber_.shutdown();
gazebo_ros_->shutdown();
alive_ = false;
queue_.clear();
queue_.disable();
// gazebo_ros_->shutdown();
callback_queue_thread_.join();
//new greag
// delete &joints_[0];
// delete &joints_[1];
joints_.pop_back();
joints_.pop_back();
joints_.empty();
ROS_INFO_NAMED("interop", "Finichild - callback_que_thread terminated, stats shutdown:%d", gazebo_ros_->ok());
}
