fix mock_driver

usdot-fhwa-stol · Jun 20, 2024 · 9e84aea · 9e84aea
1 parent 34043a8
commit 9e84aea
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 35 deletions.
diff --git a/Dockerfile b/Dockerfile
@@ -34,9 +34,9 @@
 # /////////////////////////////////////////////////////////////////////////////
 ARG DOCKER_ORG="usdotfhwastoldev"
 ARG DOCKER_TAG="develop"
-FROM 2eab02a8c0d6 as base-image
+FROM 92f7f0ade062 as base-image
 
-FROM 2eab02a8c0d6 AS source-code
+FROM 92f7f0ade062 AS source-code
 
 RUN mkdir ~/src
 COPY --chown=carma . /home/carma/src/carma-platform/
@@ -49,7 +49,7 @@ RUN ~/src/carma-platform/docker/checkout.bash -b ${GIT_BRANCH}
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM 2eab02a8c0d6 AS install
+FROM 92f7f0ade062 AS install
 ARG ROS1_PACKAGES=""
 ENV ROS1_PACKAGES=${ROS1_PACKAGES}
 ARG ROS2_PACKAGES=""
@@ -66,7 +66,7 @@ RUN ~/carma_ws/src/carma-platform/docker/install.sh
 # /////////////////////////////////////////////////////////////////////////////
 
 
-FROM 2eab02a8c0d6
+FROM 92f7f0ade062
 
 ARG BUILD_DATE="NULL"
 ARG VCS_REF="NULL"

diff --git a/carma/launch/localization.launch.py b/carma/launch/localization.launch.py
@@ -148,8 +148,8 @@ def generate_launch_description():
  {'--log-level' : GetLogLevel('basic_travel_simulator', env_log_levels) }
  ],
  remappings=[
- ("current_velocity", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
- ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/current_pose" ] ),
+ ("vehicle/twist", [ EnvironmentVariable('CARMA_INTR_NS', default_value=''), "/vehicle/twist" ] ),
+ ("current_pose", [ EnvironmentVariable('CARMA_LOCZ_NS', default_value=''), "/selected_pose" ] ),
  ("plan_trajectory", [ EnvironmentVariable('CARMA_GUIDE_NS', default_value=''), "/plan_trajectory" ] ),
  ],
  parameters=[ ]

diff --git a/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockControllerDriver.h b/mock_drivers/rosbag_mock_drivers/include/rosbag_mock_drivers/MockControllerDriver.h
@@ -27,21 +27,22 @@ namespace mock_drivers
 class MockControllerDriver : public MockDriver
 {
 private:
+ ros::NodeHandle nh_;
  boost::shared_ptr<ROSComms<cav_msgs::RobotEnabled>> robot_status_ptr_;
  ConstPtrRefROSCommsPtr<autoware_msgs::VehicleCmd> vehicle_cmd_ptr_;
- boost::shared_ptr<ROSComms<cav_srvs::SetEnableRobotic::Request&, cav_srvs::SetEnableRobotic::Response&>>
- enable_robotic_ptr_;
+ ros::ServiceServer enable_robotic_srv_;
 
  // Pub
  const std::string robot_status_topic_ = "controller/robot_status";
 
  // Sub
  const std::string vehicle_cmd_topic_ = "vehicle_cmd";
- const std::string enable_robotic_srv_ = "controller/enable_robotic";
+ const std::string enable_robotic_srv_string_ = "controller/enable_robotic";
 
  // Robot Status flags
  bool robot_active_ = false;
  bool robot_enabled_ = true; //TODO
+
 
 protected:
  int onRun() override;
@@ -63,7 +64,8 @@ class MockControllerDriver : public MockDriver
  * 
  * \return Flag idicating if the service was processed successfully. 
  */ 
- bool enableRoboticSrv(const cav_srvs::SetEnableRobotic::Request& req, cav_srvs::SetEnableRobotic::Response& res);
+ bool enableRoboticSrv(cav_srvs::SetEnableRobotic::Request& req,
+ cav_srvs::SetEnableRobotic::Response& res);
 
  // Overrides
  MockControllerDriver(bool dummy = false);

diff --git a/mock_drivers/rosbag_mock_drivers/src/MockControllerDriver.cpp b/mock_drivers/rosbag_mock_drivers/src/MockControllerDriver.cpp
@@ -33,30 +33,12 @@ void MockControllerDriver::vehicleCmdCallback(const autoware_msgs::VehicleCmd::C
  robot_enabled_ = true; // If a command was received set the robot enabled status to true
 }
 
-bool MockControllerDriver::enableRoboticSrv(const cav_srvs::SetEnableRobotic::Request& req,
+bool MockControllerDriver::enableRoboticSrv(cav_srvs::SetEnableRobotic::Request& req,
  cav_srvs::SetEnableRobotic::Response& res)
 {
- ROS_ERROR_STREAM("called enableRoboticSrv " << req.set);
+ ROS_ERROR_STREAM("called enableRoboticSrv " << static_cast<int>(req.set));
  robot_active_ = true; //set to true no matter what
  robot_enabled_ = true;
- //try
- //{
- // ROS_ERROR_STREAM("Inside TRY enableRoboticSrv " << req.set);
- // if (robot_enabled_ && req.set == cav_srvs::SetEnableRobotic::Request::ENABLE)
- // {
- // robot_active_ = true;
- // ROS_ERROR_STREAM("EAVLED IT");
- // }
- // else
- // {
- // robot_active_ = false;
- // }
- //}
- //catch(const std::exception& e)
- //{
- // ROS_ERROR_STREAM("CAught exception!!");
-//
- //}
  return true;
 }
 
@@ -71,10 +53,7 @@ MockControllerDriver::MockControllerDriver(bool dummy)
  std::bind(&MockControllerDriver::vehicleCmdCallback, this, std::placeholders::_1), CommTypes::sub, false, 10,
  vehicle_cmd_topic_);
 
- enable_robotic_ptr_ =
- boost::make_shared<ROSComms<cav_srvs::SetEnableRobotic::Request&, cav_srvs::SetEnableRobotic::Response&>>(
- std::bind(&MockControllerDriver::enableRoboticSrv, this, std::placeholders::_1, std::placeholders::_2),
- CommTypes::srv, enable_robotic_srv_);
+ enable_robotic_srv_ = nh_.advertiseService(enable_robotic_srv_string_, &MockControllerDriver::enableRoboticSrv, this);
 }
 
 bool MockControllerDriver::onSpin()
@@ -98,7 +77,7 @@ int MockControllerDriver::onRun()
  // driver publisher, subscriber, and service
  mock_driver_node_.addPub(robot_status_ptr_);
  mock_driver_node_.addSub(vehicle_cmd_ptr_);
- mock_driver_node_.addSrv(enable_robotic_ptr_);
+ //mock_driver_node_.addSrv(enable_robotic_ptr_);
 
  return 0;
 }