add setParam broadcast

ros_ws/src/crazyswarm/scripts/pycrazyswarm/crazyflie.py

@@ -158,8 +158,8 @@ def __init__(self):
  self.goToService = rospy.ServiceProxy("/go_to", GoTo)
  rospy.wait_for_service("/start_trajectory");
  self.startTrajectoryService = rospy.ServiceProxy("/start_trajectory", StartTrajectory)
- # rospy.wait_for_service("/update_params")
- # self.updateParamsService = rospy.ServiceProxy("/update_params", UpdateParams)
+ rospy.wait_for_service("/update_params")
+ self.updateParamsService = rospy.ServiceProxy("/update_params", UpdateParams)
 
  with open("../launch/crazyflies.yaml", 'r') as ymlfile:
  cfg = yaml.load(ymlfile)
@@ -194,6 +194,6 @@ def goTo(self, goal, yaw, duration, groupMask = 0):
  def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
  self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative)
 
- # def setParam(self, name, value, group = 0):
- #  rospy.set_param("/cfgroup" + str(group) + "/" + name, value)
- #  self.updateParamsService(group, [name])
+ def setParam(self, name, value):
+ rospy.set_param("/allcfs/" + name, value)
+ self.updateParamsService([name])

ros_ws/src/crazyswarm/scripts/pycrazyswarm/crazyflieSim.py

@@ -218,7 +218,7 @@ def __init__(self, timeHelper):
  self.timeHelper.crazyflies = self.crazyflies
 
  def emergency(self):
- print("WARNING: setParams not implemented in simulation!")
+ print("WARNING: emergency not implemented in simulation!")
 
  def takeoff(self, targetHeight, duration, groupMask = 0):
  for crazyflie in self.crazyflies:
@@ -239,3 +239,6 @@ def goTo(self, goal, yaw, duration, groupMask = 0):
  def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
  for crazyflie in self.crazyflies:
  crazyflie.startTrajectory(trajectoryId, timescale, reverse, relative, groupMask)
+
+ def setParam(self, name, value):
+ print("WARNING: setParam not implemented in simulation!")

ros_ws/src/crazyswarm/src/crazyswarm_server.cpp

@@ -509,6 +509,7 @@ class CrazyflieROS
 
  m_cf.logReset();
 
+ int numParams = 0;
  if (m_enableParameters)
  {
  ROS_INFO("[%s] Requesting parameters...", m_frame.c_str());
@@ -539,14 +540,15 @@ class CrazyflieROS
  ros::param::set(paramName, m_cf.getParam<float>(entry.id));
  break;
  }
+ ++numParams;
  }
  ros::NodeHandle n;
  n.setCallbackQueue(&queue);
  m_serviceUpdateParams = n.advertiseService(m_tf_prefix + "/update_params", &CrazyflieROS::updateParams, this);
  }
  auto end1 = std::chrono::system_clock::now();
  std::chrono::duration<double> elapsedSeconds1 = end1-start;
- ROS_INFO("[%s] reqParamTOC: %f s", m_frame.c_str(), elapsedSeconds1.count());
+ ROS_INFO("[%s] reqParamTOC: %f s (%d params)", m_frame.c_str(), elapsedSeconds1.count(), numParams);
 
  // Logging
  if (m_enableLogging) {
@@ -936,50 +938,50 @@ class CrazyflieGroup
  // }
  }
 
-#if 0
+
+
  template<class T, class U>
- void updateParam(uint8_t group, uint8_t id, Crazyflie::ParamType type, const std::string& ros_param) {
+ void updateParam(const char* group, const char* name, const std::string& ros_param) {
  U value;
  ros::param::get(ros_param, value);
- m_cfbc.setParam<T>(group, id, type, (T)value);
+ m_cfbc.setParam<T>(group, name, (T)value);
  }
 
  void updateParams(
- uint8_t group,
  const std::vector<std::string>& params)
  {
  for (const auto& p : params) {
- std::string ros_param = "/cfgroup" + std::to_string((int)group) + "/" + p;
+ std::string ros_param = "/allcfs/" + p;
  size_t pos = p.find("/");
  std::string g(p.begin(), p.begin() + pos);
  std::string n(p.begin() + pos + 1, p.end());
 
- // TODO: this assumes that all IDs are identically
- //  should use byName lookup instead!
+ // This assumes that we can find the variable in the TOC of the first
+ // CF to find the type (the actual update is done by name)
  auto entry = m_cfs.front()->getParamTocEntry(g, n);
  if (entry)
  {
  switch (entry->type) {
  case Crazyflie::ParamTypeUint8:
- updateParam<uint8_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<uint8_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeInt8:
- updateParam<int8_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<int8_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeUint16:
- updateParam<uint16_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<uint16_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeInt16:
- updateParam<int16_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<int16_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeUint32:
- updateParam<uint32_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<uint32_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeInt32:
- updateParam<int32_t, int>(group, entry->id, entry->type, ros_param);
+ updateParam<int32_t, int>(g.c_str(), n.c_str(), ros_param);
  break;
  case Crazyflie::ParamTypeFloat:
- updateParam<float, float>(group, entry->id, entry->type, ros_param);
+ updateParam<float, float>(g.c_str(), n.c_str(), ros_param);
  break;
  }
  }
@@ -988,7 +990,6 @@ class CrazyflieGroup
  }
  }
  }
-#endif
 
 private:
 
@@ -1267,7 +1268,7 @@ class CrazyflieServer
  m_serviceLand = nh.advertiseService("land", &CrazyflieServer::land, this);
  m_serviceGoTo = nh.advertiseService("go_to", &CrazyflieServer::goTo, this);
 
- // m_serviceUpdateParams = nh.advertiseService("update_params", &CrazyflieServer::updateParams, this);
+ m_serviceUpdateParams = nh.advertiseService("update_params", &CrazyflieServer::updateParams, this);
 
  m_pubPointCloud = nh.advertise<sensor_msgs::PointCloud>("pointCloud", 1);
 
@@ -1779,7 +1780,6 @@ class CrazyflieServer
  return true;
  }
 
-#if 0
  bool updateParams(
  crazyflie_driver::UpdateParams::Request& req,
  crazyflie_driver::UpdateParams::Response& res)
@@ -1788,14 +1788,14 @@ class CrazyflieServer
 
  for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
  for (auto& group : m_groups) {
- group->updateParams(req.group, req.params);
+ group->updateParams(req.params);
  }
  std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
  }
 
  return true;
  }
-#endif
+
 //
  void readMarkerConfigurations(
  std::vector<libobjecttracker::MarkerConfiguration>& markerConfigurations)