This is my code for the Adaptive Monte Carlo localization(AMCL) algorithm using ROS.
#include <ignition/plugin/Register.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Util.hh>
#include <ignition/gazebo/System.hh>
//ROS libraries
#include <ros/ros.h>
#include <ros/callback_queue.h>
// Dependencies
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
// STL
#include <memory>
#include <thread>
#include <string>
using namespace ignition;
using namespace gazebo;
using namespace systems;
////////////////////////////////////////////////////////////////////////////
class ignition::gazebo::systems::amclPrivate
{
public: Model model{kNullEntity};
public: bool updateAmclPose(const UpdateInfo &_info,
const EntityComponentManager &_ecm);
};
/////////////////////////////////////////////////////////////////////////////
amcl::amcl() :
System(), dataPtr(std::make_unique<amclPrivate>())
{
tfListener_(tfBuffer_)
}
/////////////////////////////////////////////////////////////////////////////
amcl::~amcl() = default;
{
// Disable callback queue
amcl_queue_.clear();
amcl_queue_.disable();
// Shutdown ROS node handle
amcl_nh_.shutdown();
// Wait for child thread to join
if(thread_ptr_->joinable())
thread_ptr_->join();
}
/////////////////////////////////////////////////////////////////////////////
void amcl::Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &_eventMgr)
{
this->dataPtr->model = Model(_entity);
if (!this->dataPtr->model.Valid(_ecm))
{
ignerr << "amcl plugin should be attached to a model entity. "
<< "Failed to initialize." << std::endl;
return;
}
// Obtain parameters from xacro file
if(_sdf->HasElement("topicRate"))
{
rate_ = _sdf->GetElement("topicRate")->Get<double>(); ########################line 95
}
else
{
rate_ = 10.0;
}
if(_sdf->HasElement("fixedFrame"))
{
fixed_frame_ = _sdf->GetElement("fixedFrame")->Get<std::string>(); ############################line 107
}
else
{
fixed_frame_ = "map";
}
if(_sdf->HasElement("robotFrame"))
{
robot_frame_ = _sdf->GetElement("robotFrame")->Get<std::string>(); ######################line 118
}
else
{
robot_frame_ = "base_link";
}
// Ensure that ROS node for Gazebo has been initialized
if(!ros::isInitialized())
{
int argc = 0; char ** argv = NULL;
ros::init(argc, argv, "ignition_gazebo_robot", ros::init_options::NoSigintHandler);
}
// Set callback queue
amcl_nh_.setCallbackQueue(& amcl_queue_);
amcl_pub_ = amcl_nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl",1,true);
thread_ptr_ = std::make_shared<std::thread>([this]()
{
// Set process rate
ros::Rate r(this->rate_);
while (this->amcl_nh_.ok())
{
this->updateAmclPose(const UpdateInfo &_info,
const EntityComponentManager &_ecm);
this->amcl_pub_.publish(this->msg_);
this->amcl_queue_.callAvailable(ros::WallDuration(0.0));
r.sleep();
}
}
// Notify plugin status
ROS_INFO_STREAM(
ros::this_node::getName() << " plugin amcl pose successfully loaded."
);
}
//////////////////////////////////////////////////////////////////////////////////////
void amcl::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm)
{
IGN_PROFILE("amcl::PreUpdate");
if (_info.dt < std::chrono::steady_clock::duration::zero())
{
ignwarn << "Detected jump back in time ["
<< std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count()
<< "s]. System may not work properly." << std::endl;
}
///////////////////////////////////////////////////////////////////////////////////////
void amcl::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm)
{
IGN_PROFILE("amcl::PostUpdate");
// Nothing left to do if paused.
if (_info.paused)
return;
this->dataPtr->updateAmclPose(const UpdateInfo &_info,const EntityComponentManager &_ecm)
///////////////////////////////////////////////////////////////////////////////////////
bool amcl::updateAmclPose(const UpdateInfo &_info,
const EntityComponentManager &_ecm)
{
IGN_PROFILE("amcl::updateAmclPose");
bool isok {false};
try
{
if(tfBuffer_.canTransform(fixed_frame_, robot_frame_, ros::Time(0)))
{
read_transformation_ = tfBuffer_.lookupTransform(fixed_frame_, robot_frame_, ros::Time(0), ros::Duration(60.0));
msg_.header.frame_id = fixed_frame_;
msg_.header.stamp = ros::Time::now();
msg_.pose.pose.position.x = read_transformation_.transform.translation.x;
msg_.pose.pose.position.y = read_transformation_.transform.translation.y;
msg_.pose.pose.position.z = read_transformation_.transform.translation.z;
msg_.pose.pose.orientation.x = read_transformation_.transform.rotation.x;
msg_.pose.pose.orientation.y = read_transformation_.transform.rotation.y;
msg_.pose.pose.orientation.z = read_transformation_.transform.rotation.z;
msg_.pose.pose.orientation.w = read_transformation_.transform.rotation.w;
}
isok = true;
}
catch(const tf2::TimeoutException & e)
{
ROS_ERROR_STREAM(
ros::this_node::getName() << " " << __func__ <<
" timeout exception: " << e.what()
);
return isok;
}
catch(const tf2::TransformException & e)
{
ROS_ERROR_STREAM(
ros::this_node::getName() << " " << __func__ <<
" transform exception: " << e.what()
);
return isok;
}
return isok;
}
}
}
Error (in line 95, 107 and 118)
error: passing ‘std::__shared_ptr_access<const sdf::v9::Element, (__gnu_cxx::Lock_policy)2, false, false>::element_type’ {aka ‘const sdf::v9::Element’} as ‘this’ argument discards qualifiers [-fpermissive] rate = _sdf->GetElement("topicRate")->Get();
Complete error (as shown in the terminal)
/home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc: In member function ‘virtual void ignition::gazebo::v3::systems::amcl::Configure(const Entity&, const std::shared_ptr&, ignition::gazebo::v3::EntityComponentManager&, ignition::gazebo::v3::EventManager&)’: /home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc:95:45: error: passing ‘std::__shared_ptr_access<const sdf::v9::Element, (__gnu_cxx::Lock_policy)2, false, false>::element_type’ {aka ‘const sdf::v9::Element’} as ‘this’ argument discards qualifiers [-fpermissive] rate = _sdf->GetElement("topicRate")->Get(); ^ In file included from /usr/include/ignition/gazebo3/ignition/gazebo/System.hh:28, from /home/adwaitnaik/plugin_ws/src/amcl/Amcl.hh:18, from /home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc:10: /usr/include/sdformat-9.5/sdf/Element.hh:330:24: note: in call to ‘sdf::v9::ElementPtr sdf::v9::Element::GetElement(const string&)’ public: ElementPtr GetElement(const std::string &_name); ^~~~~~~~~~ /home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc:107:53: error: passing ‘std::__shared_ptr_access<const sdf::v9::Element, (__gnu_cxx::Lock_policy)2, false, false>::element_type’ {aka ‘const sdf::v9::Element’} as ‘this’ argument discards qualifiers [-fpermissive] fixed_frame = _sdf->GetElement("fixedFrame")->Getstd::string(); ^ In file included from /usr/include/ignition/gazebo3/ignition/gazebo/System.hh:28, from /home/adwaitnaik/plugin_ws/src/amcl/Amcl.hh:18, from /home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc:10: /usr/include/sdformat-9.5/sdf/Element.hh:330:24: note: in call to ‘sdf::v9::ElementPtr sdf::v9::Element::GetElement(const string&)’ public: ElementPtr GetElement(const std::string &_name); ^~~~~~~~~~ /home/adwaitnaik/plugin_ws/src/amcl/Amcl.cc:118:53: error: passing ‘std::__shared_ptr_access<const sdf::v9::Element, (__gnu_cxx::Lock_policy)2, false, false>::element_type’ {aka ‘const sdf::v9::Element’} as ‘this’ argument discards qualifiers [-fpermissive] robot_frame = _sdf->GetElement("robotFrame")->Getstd::string(); ^
The line is marked in the code.
I referred to this link but couldn't get the solution
Can anybody suggest a workaround?
PS: I got the same error for line 107 and 118 (marked in code with '#######')