// 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()); }